COMMENT ⊗   VALID 00063 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00005 00002		TITLE ARM
C00008 00003		These are conditional assembly flags
C00010 00004	IFN SIMU,<
C00014 00005		THIS IS THE 32 WORD MAIL MESSAGE
C00015 00006	START
C00018 00007	EXFN:	CAIN 1,3
C00020 00008		SKIPE BAND
C00021 00009		MOVE 1,FILE
C00022 00010	ZDTH:
C00025 00011	BACK:	
C00028 00012	SETNAM:	MOVE 2,THETA
C00029 00013	IFN REPORT+TRACK,<
C00030 00014	READ:	SETZM STWT
C00031 00015	SPACE:	MOVEI 1,=180
C00033 00016	FINISH:	SPCWAR 636367
C00035 00017	SPACEWAR CODE
C00038 00018	NXTJIF:
C00040 00019	WATSTL:
C00042 00020	IFN ADJUST,<
C00043 00021	MAIN:	SKIPE MOVT		NO MOVING RESTORE
C00046 00022	TT:	PUSHJ P,REFLEX
C00047 00023	MEASURE JOINT ANGLE
C00049 00024	MEASURE JOINT VELOCITY
C00050 00025	SERVO:	TRNN RUN
C00052 00026	DRIVE THE ARM
C00055 00027	STOP ON FORCE
C00058 00028	WIPER:	0
C00059 00029	NXTH:	MOVE AC,[XWD T0,T0P]
C00063 00030	CHECK THE SET POINT LIMIT
C00064 00031	EVAL:	MOVEI I,5
C00069 00032	DHO:	AOJ I,			DISASTER HAS OCCURED
C00072 00033	NEXT:	TRNE RUN
C00075 00034	SETSET:	MOVEI I,5
C00078 00035	MOTOR DRIVE FUNCTIONS
C00080 00036	PREAD:
C00082 00037	DATA
C00089 00038	DYNAMICS
C00091 00039	NON LINEAR CALIBRATION
C00097 00040	HOPEN:				OPENS THE HAND TO SET POINT
C00100 00041	HCLOSE:
C00102 00042	HEAD:	MOVEI K,HANDCHA
C00105 00043	PLACE:				PLACE HAND ON TABLE
C00106 00044	NUDGE:	PUSHJ P,ACSAVE
C00108 00045	DRIVE:	HRRE TAC,@STKPTR
C00109 00046	STOP:	HRRE 1,@STKPTR
C00110 00047	TOOL:	TRNN HCL
C00111 00048	VOPEN:	MOVEI	TAC,1000
C00113 00049	PATH:	TRO RUN
C00115 00050	ASSIGN:	HRRE K,@STKPTR
C00116 00051	SAVE:	HRRE K,@STKPTR
C00119 00052	SET.ARM:HRRE K,@STKPTR
C00120 00053	CENTER:	TRNN HCL
C00123 00054	WOBBLE:	HRLZ AC,@STKPTR
C00124 00055	SEA:	SETZM XSEL
C00127 00056	SOJGI:	HRRE K,@STKPTR
C00128 00057	MOVING: HRRE K,@STKPTR
C00129 00058	HOME:	HRRE AC,@STKPTR
C00133 00059	IKU:	HRRE AC,@STKPTR
C00135 00060	REFLEX:	MOVEI I,1
C00136 00061	SETCH:	MOVE AC,SOTCH
C00137 00062	RETURN:	SETOM RETFLG
C00138 00063		XLIST
C00139 ENDMK
C⊗;
	TITLE ARM
;	These are the error messages returned by the arm servo program

	ALLOK←←0	;ALL OK
	NOFILE←←3	;FILE NOT FOUND
	NODATA←←4	;ENTER ON DATA FILE FAILED
	NODEV←←5	;DEVICE DSK DOES NOT EXIST
	EXER←←1		;IF JUST 1 THEN OVERFLOW OR UNDERFLOW OCCURED,
	;IF OF THE FORM X1 THEN EXCESSIVE FORCE REQUIRED AT JOINT X
	GRASER←←2	;GRASP ERROR, HAND CLOSED TOO FAR
	TOUCH←←6	;STOPPED ON TOUCH
	;THIS ERROR IS OF THE FORM X6 WHERE THE X'th SENSOR TOUCHED
	READER←←7	;A/D READ ERROR
	TERR←←20	;SPACEWAR JOB FAILS TO TERMINATE
	HTERR←←22	;HAND FAILS TO FINISH IN TIME
	STPERR←←23	;ARM FAILED TO STOP ON FORCE LIMIT
	LOOPER←←24	;ARM IN CONTROL LOOP
	SAVERR←←25	;SAVE ARRAY INDEX ERROR
	FUNERR←←27	;FUNCTION DOES NOT EXIST
	NOTHER←←30	;THE ARM IS DOWN
	INVMES←←40	;MESSAGE INVALID, ARM NOT IN WAIT STATE
	REAERR←←50	;TRAJECTORY FILE READ ERROR
	VERSN←←60	;VERSION NUMBER WRONG
	REFERR←←70	;REFERENCE SUPPLY IS OFF,CHECK THE FUSE
	PDPERR←←100	;PDP-6 IS DOWN
	ISERR←←200	;PDP-6 SERVICE INTERRUPTED
	ADBUS←←300	;AD IS BUSY
	XGPBUS←←400	;XGP IS BUSY
	SOLERR←←500	;SOLUTION DOES NOT EXIST
	SOJERR←←600	;SOJG COUNT DOES NOT EXIST 
	NOSERV←←700	;OTHER SERVO NOT RUNNING
	INVPRO←←702	;INVALID PROCEED, RUN SEGMENTS DO NOT MATCH
	PAUSER←←703	;NO RESPONSE IN PAUSE WAIT
	REDOFF←←1000	;RED BUTTON HAS BEEN PUSHED

	XLIST
	SUBTTL	AC DEFINITIONS AND FLAGS			LOU PAUL
	LIST
;	These are conditional assembly flags
	BLUE←←0		;assemble for blue arm
	DEB←←0		;causes the AC's to be saved each jiffy
	TACH←←0		;Integrates tach voltage and sets vgain
	SMV←←0		;measures max and min tach dac readings
	CALIB←←0	;Sets offsets for multiple wiper pots
	TRACK←←0	;Causes a debugging file to be written
IFE TRACK,<
	DYNAMICS←←0	;CAUSES FORWARD AND FEEDBACK TO BE WRITTEN OUT
	REPORT←←0+DYNAMICS	;Causes display data to be written
>
IFN TRACK,<
	REPORT←←0
	DYNAMICS←←0
>
	SIMU←←0		;Turns this program into a sail callable routine to
			;simulate the arm
	USER←←0+SIMU	;This runs the SPACEWA1R job in user mode and simulates the arm
	ADJG←←0			;READS THE POT BOX TO SET SERVO GAINS
IFN ADJG,<ADJD←←0>
IFE ADJG,<
	ADJD←←0			;READS THE POT BOX TO SET DRIVE FACTORS
>
	ADJUST←←ADJG+ADJD	;CAUSES THE POTS TO BE READ TO ADJUST GAINS
	HANDWORK←←1		;IF ZERO DOSN'T DRIVE THE HAND
	ARMWORK←←1		;IF ZERO RETURNS THE ARM DOWN MESSAGE
IFN SIMU,<
	INTERNAL ARM.INIT,ARM.STEP
	INTERNAL ARM.THETA,ARM.GRASP,ARM.TOUCH,ARM.FUNCTION
>
	EXTERNAL JOBREN,JOBOPC,SOLVE,UPDATE,TIMES
IFN ADJUST,<EXTERNAL EXP>
	OPDEF INTUUO [723B8]
	OPDEF UFBRD [706B8]

;	AC'S
	BITS←←0
	AC←1
	TAC←2
	MQ←3
	I←5
	J←6
	K←7
	L←10
	DACVAL←11	;CONTAINS THE A/D READING WITH THE SIGNBITS COMP'D
	CBUF←12		;POINTS TO CURRENT BUFFER
	DATWD←13	;ARM DATAO
	DATA←14		;DATA POINTER FOR DEBUGGING BUFFER
	TAC1←15
	P←17		;PUSH DOWN POINTER


;	CONSTANTS
	PDLL←←20	;LENGTH OF PUSH DOWN
	MASTER←←16661	;VERSION NUMBER
IFN BLUE,<MASTER←←MASTER+1>
	DBL←←4000	;LENGTH OF DEBUGGING BUFFER
	AD←←424		;DEVICE AD
	DB←←204		;DEVICE 136
	CLOCK←←730	;DEVICE CLOCK
	DEVT←←450	;DEVICE TOOL
IFE BLUE,<
	CHA←←62B23	;JOINT 6 POT CHANNEL
	CHA5←←61B23	;JOINT 5 POT CHANNEL
	N6CHA←←63B23	;ALT JOINT 6 POT CHANNEL
	HANDCHA←←70B23	;HAND POT CHANNEL
	ARM←←420	;DEVICE ARM
	WIDTH←←600	;D/A TO SET PULSE WIDTHS
>
IFN BLUE,<
	CHA←←36B23	;JOINT 6 POT CHANNEL
	CHA5←←35B23	;JOINT 5 POT CHANNEL
	N6CHA←←37B23	;ALT JOINT 6 POT CHANNEL
	HANDCHA←←40B23	;HAND POT CHANNEL
	ARM←←450	;DEVICE ARM
>
	MAXFN←←33	;MAXIMUM NUMBER OF FUNCTIONS
	JMPABS←←100	;JUMP ABSOLUTE
	SKPERR←←101	;SKIP ON ERROR
	SKPNER←←103	;SKIPN ON ERROR
	SKPSER←←104	;SKIPS ON ERROR
	JMPREL←←102	;JUMP RELATIVE
	NSCEL←←=10	;NUMBER SAVE CELLS
	NLCEL←←=10	;NUMBER ASSIGN CELLS
	WOJC←←=240	;WAIT 60 SECONDS

IFN USER,<OPDEF DATAO [JFCL]>

;	FLAGS WHICH RESIDE IN AC 0 OR IN BITSAV

	DTST←←1		;SPACE WAR JOB FINISHED
	POTREAD←←2	;READ POTS IN OCTAL+=1000, DO NOT CONVERT TO DEGREES
	RONLY←←4	;JUST READ POTS
	FINAL←←10	;NULL ARM ERRORS
	RUN←←20		;RUN ARM
	INCREM←←40	;INCREMENT SET POINT TOWARD DTH
	DROP←←100	;PLACE ARM DO NOT STOP IF ERRORS ARE ZERO
	ALT6←←200	;READ ALT POT 6
	HCL←←400	;INITIALIZATION FOR HAND FUNCTIONS
	ALT←←1000	;TRAJECTORY ALT PATH
	STP←←2000	;STOP ON FORCE LIMIT
	TRIP←←4000	;TURN ON ARM
	REREAD←←10000	;AD HAS HAD TO BE RESTARTED
	STCH←←20000	;STOP ON TOUCH
	PROFLG←←40000	;ARMPROCEED
	WOB←←100000	;WOBBLE OUTER JOINTS
	HOMEF←←200000	;FUNCTION IS HOME
	BOTH←←400000	;MERGE
;FLAGS WHICH RESIDE IN THE LH OF DATWD
	NNUL←←1		;DO NOT NULL
	INT←←2		;INTERPOLATE CI AS THE ARM MOVES
	NXTFN←←4	;START NEXT FUNCTION NOW
;FLAGS IN LH OF INSTRUCTIONS
	MERGE←←400	;MERGE

	RELOCF←←40
	HOLDF←←20
	DEC←←-4
	XF←←1

	XALL
	XLIST
	SUBTTL	MESSAGE FORMAT				LOU PAUL
	LIST
;	THIS IS THE 32 WORD MAIL MESSAGE

EXE:	XWD MASTER,0	;18-27 NARGS, 30 RELOC, 31 HOLD, 32 SOTCH, 33-35 EXE
			;0 READ IN OCTAL, returns pot reading in octal with
			;  no non-linear calibration.  sets potread
			;1 READ IN DECIMAL
			;2 EXECUTE FILE
			;3 EXECUTE FUNCTION, its a DO ____
			;  function overwrites message block 
			;4 PROCEED FROM OTHER SERVO
FILE:	0
PPPN:	0
BAND:	0
SEGJOB:	0
NAME:	0
STATUS:	0
SEGMENT:0
NERVES:	0
ETOR:	0
THETA:	0
ARGS:	BLOCK =6
STRING:	BLOCK =14
SAVE.STRING:0
	XLIST
	SUBTTL	USER LEVEL CODE				LOU PAUL
	LIST
;START
IFN SIMU,<			;THIS IS SIMULATION ONLY
ARM.IN:	
IFE BLUE,<MOVE 1,[SIXBIT/ARM-Y/]>
IFN BLUE,<MOVE 1,[SIXBIT/ARM-B/]>
	CALLI 1,400002
	SETZM INRUN
	SETOM ARMINI		;INDICATES ARM.IN HAS BEEN CALLED
ARM.ST:	SETO 1,			;INDICATES END OF RUN
	SKIPN ARMINI
	POPJ P,
	MOVEM 16,SSAVE+16	;SAVE SAIL AC'S EXCEPT FOR STACK
	MOVEI 16,SSAVE
	BLT 16,SSAVE+15
	SKIPE  INRUN
	JRST[	HRLZI 16,MSAVE	;PUT BACK IN RUN AC'S
		BLT 16,15
		MOVE 16,MSAVE+16
		JRST SSW]	;AND RETURN TO SPACE WAR
	JRST GETMES
INRUN:	0			;IN THE MIDDLE OF A RUN DO NEXT TICK
ARMINI:	0			;ARM.IN HAS BEEN CALLED
SSAVE:	BLOCK 17		;SAIL AC'S
MSAVE:	BLOCK 17		;USER AC'S
WAIT:	HRLZI 16,SSAVE		;PUT BACK AC'S
	BLT 16,15
	MOVE 16,SSAVE+16
	SETZM INRUN
	POPJ P,
GETMES:	SETOM INRUN
>
IFE SIMU,<
START:	MOVE P,[XWD -PDLL,UPDL]
	SETZM LCOUNT
	MOVE 2,[XWD LCOUNT,LCOUNT+1]
	BLT 2,LNAME+NLCEL-1
IFE BLUE,<MOVE 1,[SIXBIT/ARM-Y/]>
IFN BLUE,<MOVE 1,[SIXBIT/ARM-B/]>
	CALLI 1,400002
	OUTSTR[ASCIZ/!
/]
WAIT:
>
	WRCV EXE
	HLRZ 5,EXE
	CAIE 5,MASTER
	JRST[	MOVEI 3,VERSN
		JRST FINISH]
WAITED:	SETZB 3,TRAJER
	MOVEI 0,0
	MOVE 1,EXE
	ANDI 1,7
	CAIN 1,4
	JRST[	MOVE 1,ARGS	;runseq number from other servo
		CAMN 1,RUNSEG
		JRST MOVON
		MOVEI 3,INVPRO
		JRST FINISH]
	MOVE 1,SEGJOB
	HRRZM 1,JOB
	HLRZM 1,SEG
IFN REPORT+TRACK,<
	MOVE 1,[IOWD  DBL+100,DATO]
	MOVEM 1,DATPT			;DATA POINTER
	MOVEM 1,DATIWD			;OUTPUT POINTER
>
	LDB 1,[POINT 3,EXE,35]
	CAIG 1,1
	JRST READ
IFE ARMWORK,<
	MOVEI 3,NOTHER
	JRST FINISH
>
IFN REPORT+TRACK,<		;WRITE OUT DATA
	INIT 15,17
	SIXBIT /DSK/
	0
	JRST NDM
	MOVE 2,FILE
	MOVEM 2,DFILE
	MOVE 2,PPPN
	MOVEM 2,DPPPN
IFN TRACK,<
	SKIPE WALK
	JRST[	LOOKUP 15,DFILE
		JRST NDMS
		MOVE 2,[IOWD DBL,DATO]
		MOVEM 2,DATPT
		INPUT 15,1
		JRST EXFN]
>
	ENTER 15,DFILE
	JRST[NDMS:MOVEI 3,NODATA
		SUB 17,[XWD 1,1]
		JRST FINISH]
>
EXFN:	CAIN 1,3
	JRST[	MOVE 4,EXE		;EXECUTE FUNCTION
		TRNN 4,HOLDF		;EXECUTE AND WAIT?
		JRST [	SKIPE ARGS	;IF ARGS THEN REPEAT LAST INSTRUCTION
			SOS JUMPBK	;BY DECREMENTING RETURN ADDRESS
			JRST MOVON]	;AND PROCEEDING
		SOS 1,STKPTR		;WHERE TO PUT INSTRUCTION
		MOVEI 2,(1)		;ADDRESS OF INSTRUCTION CELL
		LDB 3,[POINT 9,EXE,26]	;NUMBER OF WORDS OF DATA FOR INSTRUCTION
		SUBI 1,(3)		;PUT THIS BEFORE THE INSTRUCTION
		HRLI 1,ARGS		;ADDRESS OF DATA IN MESSAGE
		BLT 1,(2)
		TRNE 4,RELOCF		;IS THIS A RELOCATABLE INSTRUCTION?
		JRST[	MOVE 1,STKPTR	;YES! THIS IS WHERE THE INSTRUCTION IS
			SUBI 1,(3)	;WHERE THE DATA IS
			SUB 1,CBUFS	;BASE ADDRESS
			HRRM 1,@STKPTR	;PUT IT IN THE ADDRESS FIELD 
			JRST BPTR]
	BPTR:	SOS STKPTR		;DECREMENT THE STACK POINTER
	MOVON:	SETOM FUNCT		;SET THE FUNCTION INVALID
		JRST SSW]		;AND PROCEED
	CAIE 1,2
	JRST[	MOVEI 3,INVMES		;INVALID MESSAGE
		JRST FINISH]
	SETZM FUNCT			;READ
	HRREI 2,BUF-1001
	MOVEM 2,CBUFS
	SETOM BUF
	SETZM DOING	;indicates segment of trajectory being executed
	SETZM RUNSEG	;reset pause count, used with proceeds

	SKIPE BAND
	JRST[	MOVEI 1,BUF
		MOVEM 1,GOODIE
		MOVEI 1,1001
		MOVEM 1,GOODIE+1
		SETZM GOODIE+2
		MOVE 14,BAND
		UFBRD 14,GOODIE
		JRST RE
		SKIPN BUF+1000
		JRST[	SETZM BLOCKS
			MOVEI 1,BUF2
			MOVEM 1,NXTBUF
			SETZM @NXTBUF
			JRST ZDTH]
		MOVEI 1,BUF2
		MOVEM 1,GOODIE
		MOVEI 1,20
		MOVEM 1,GOODIE+2
		UFBRD 14,GOODIE
		JRST RE
		MOVEI 1,=74
		SKIPN BUF2+1000
		MOVEI 1,0
		MOVEM 1,BLOCKS
		MOVEI 1,40
		MOVEM 1,GOODIE+2
		MOVEI 1,BUF
		MOVEM 1,NXTBUF
		MOVNS BUF2
		JRST ZDTH]
	MOVE 1,FILE
	MOVEM 1,LFILE
	MOVE 1,PPPN
	MOVEM 1,LPPPN
	INIT 16,17
	SIXBIT/DSK/
	XWD 0,0
	JRST[NDM:	MOVEI 3,NODEV
			JRST FINISH]
	LOOKUP 16,LFILE
	JRST[		MOVEI 3,NOFILE
			JRST FINISH]
	MOVS 1,LPPPN		;FILE LENGTH
	MOVN 1,1
	ASH 1,-11
	MOVEM 1,BLOCKS
	SOSGE BLOCKS
	JRST FINISH
	MOVE 10,[IOWD 1000,BUF]
	SETZ 11,
	IN 16,10
	JRST[	SOSGE BLOCKS
		JRST[	MOVEI 1,BUF2
			MOVEM 1,NXTBUF
			SETZM @NXTBUF
			JRST ZDTH]
		ADDI 10,1001
		IN 16,10
		JRST[	MOVEI 1,BUF
			MOVEM 1,NXTBUF
			MOVNS BUF2
			JRST ZDTH]
		JRST RE]
	JRST RE
ZDTH:
	MOVE 1,[XWD DELTH,DELTH+1]
	SETZM DELTH
	BLT 1,DELTH+5
	MOVEI 1,=60
	MOVEM 1,STWT
	MOVNS BUF
	HLRZ 1,BUF
	CAIE 1,MASTER
	JRST[	MOVEI 3,VERSN
		JRST FINISH]
SSW:	
	PUSHJ P,SPACE
WWS:
IFE REPORT+TRACK<
IFE USER<
	PUSH P,[SIXBIT/RUN/]
	PUSHJ P,MSGCHG
>
>
	MOVE BITSAV
	TRNE DTST
	JRST BACK
	MOVE 1,@NXTBUF
	JUMPGE 1,.+2			;MORE TRAJECTORY TO COME
	JRST[	SOSGE BLOCKS
		JRST[	SETZM @NXTBUF	;ALL DONE MAKK THIS AS LAST
			RELEASE 16,
			JRST WDO]
		SKIPE BAND
		JRST[	MOVE 1,NXTBUF
			MOVEM 1,GOODIE
			UFBRD 14,GOODIE
			JRST RBD
			MOVE 1,NXTBUF
			SKIPN 1000(1)
			SETZM BLOCKS
			MOVEI 1,20
			ADDM 1,GOODIE+2
			JRST ROK]
		MOVE 10,NXTBUF
		SOJ 10,
		HRLI 10,-1000
		IN 16,10
		JRST[ROK:MOVE 1,NXTBUF
			MOVNS (1)
			ADDI 1,1001
			CAILE 1,BUF2
			MOVEI 1,BUF
			MOVEM 1,NXTBUF
			JRST WDO]
	RBD:	MOVE 1,NXTBUF		;READ ERROR
		SETZM @NXTBUF		;MARK AS LAST
	WFF:	MOVE BITSAV		;AND WAIT FOR IT TO END
		TRNN DTST
		JRST WFF
	RE:	MOVEI 3,REAERR
		JRST BACK]
WDO:
IFN SIMU,<
	MOVEM 16,MSAVE+16
	MOVEI 16,MSAVE
	BLT 16,MSAVE+15
	HRLZI 16,SSAVE
	BLT 16,15
	MOVE 16,SSAVE+16
	SETZ 1,
	POPJ P,
>
IFE REPORT+TRACK,<
	IFE USER,<JRST WWS>
	IFN USER,<JRST SSW>
>
IFN TRACK,<
	SKIPE WALK
	JRST SSW
>
IFN REPORT+TRACK,<
	HRRZ 1,DATIWD		;START TO WRITE
	CAIL 1,DATO+DBL-1
	JRST DOD
	HRRZ 2,DATPT		;LAST LOC FILLED
WA:	SUBI 2,(1)
	JUMPL 2,[MOVEI 2,DATO+DBL-1
	JRST WA]
	TRZ 2,177
	JUMPN 2,[	MOVN 1,2	;MORE THAN 200 WORDS SO WRITE IT OUT
			HRLM 1,DATIWD	;NEG WORD COUNT
			OUT 15,DATIWD
			HRRZ 1,DATIWD
			ADDI 1,(2)
			HRRM 1,DATIWD
			JRST DOD]
DOD:
	IFE USER,<JRST WWS>
	IFN USER,<JRST SSW>
>
BACK:	
IFE USER+TRACK,<UNLOCK 1,>
	SPCWAR 636367
	SKIPE 3,TRAJER
	JRST[	IFE DEB+SIMU,<
		IFN TRACK,<SKIPN WALK>
			MOVEI 0,0
			TRO RONLY
			MOVEM BITSAV
			PUSH P,[SIXBIT/REREAD/]
			PUSHJ P,MSGCHG
		DRSW:	PUSHJ P,SPACE
		DRR:	MOVE BITSAV
			TRNN DTST
		IFE USER,<JRST DRR>
		IFN USER,<JRST DRSW>
			SPCWAR 636367
			IFE USER+TRACK,<UNLOCK 1,>
			MOVEI 0,0
			PUSHJ P,SETSET
>
			RELEASE 17,
			RELEASE 14,
			JRST NOERR]
	RELEASE 17,
	RELEASE 14,
	TRZE PROFLG
	JRST[
IFE BLUE,<MOVE 1,[SIXBIT/ARM-B/]>
IFN BLUE,<MOVE 1,[SIXBIT/ARM-Y/]>
		CALLI 1,400043
		JRST NOERR
		SKIPE PAUSEF
		JRST[	SETZM PAUSEF
			JRST HEREW]
		MOVE 1,RUNSEG
		MOVEM 1,ARGS
		MOVEI 1,4
		DPB 1,[POINT 3,EXE,35]
		SEND PROREQ
		JRST[	MOVEI 3,NOSERV
			JRST FINISH]
		SKIPE RETFLG
		JRST[	SETZM RETFLG
			JRST NOERR]
	HEREW:	MOVEI 2,WOJC
	HEREI:	PUSH P,[SIXBIT/PAUSE/]
		PUSHJ P,MSGCHG		;SLEEP 1 SECOND
		SRCV EXE		;IS THERE A MESSAGE YET?
		JRST[	SOJGE 2,HEREI	;NO WAIT SOME MORE
			MOVEI 3,PAUSER		;PAUSE ERROR
			JRST FINISH]
		AOS RUNSEG
		JRST WAITED]
	MOVEI 0,0
IFN SIMU,<PUSHJ P,SETSET>
NOERR:	
	IFN REPORT+TRACK,<
	PUSHJ P,CLODAT
	CLOSE 15,
	>
	SKIPN SEG
	JRST[	MOVEM 3,STATUS
		MOVE 2,[XWD TH,THETA]
		BLT 2,THETA+6
		MOVE 1,JOB
		MOVEM 1,REPLY
		SEND REPLY
		JRST WAIT
		JRST WAIT]
	MOVE 2,SEG
	CALLI 2,400016
	JRST WAIT
	HRRZ 4,NAME
	MOVE 2,(4)
	HLRZ 4,NAME
	MOVEM 2,(4)
	PUSHJ P,SETNAM
	JRST WAIT

SETNAM:	MOVE 2,THETA
	MOVEI 4,6(2)
	HRLI 2,TH
	IFN TRACK,<SKIPN WALK>
	BLT 2,(4)
	HRRZ 2,ETOR
	MOVEI 4,=14(2)
	HRLI 2,STRING
	BLT 2,(4)
	PUSHJ P,SETFLT
	MOVEM 3,@STATUS
	MOVE 4,DOING
	MOVEM 4,@SEGMENT
	HRRZ 2,NAME
	SETZM (2)
	HRLZI 2,40
	MOVEM 2,IBIT
	INTUUO 4,JOB
	JUMP
	MOVEI 2,0
	CORE2 2,
	JFCL
	POPJ P,
IFN REPORT+TRACK,<
CLODAT:IFN TRACK,<
	SKIPE WALK
	POPJ P,
>
NWI:	HRRZ 1,DATIWD
	HRRZ 2,DATPT
	SUBI 2,(1)
	JUMPL 2,[	HRRZI 2,DATO+DBL-1
			SUBI 2,(1)
			MOVN 2,2
			HRLM 2,DATIWD
			SKIPE 2
			OUT 15,DATIWD
			MOVEI 2,DATO-1
			HRRM 2,DATIWD
			JRST NWI]
	ADDI 2,200
	TRZ 2,177
	MOVN 1,2
	HRLM 1,DATIWD
	OUT 15,DATIWD
	POPJ P,
>

READ:	SETZM STWT
	SKIPG 1
	TRO  POTREAD
	TRO RONLY
RSW:	PUSHJ P,SPACE
RSWW:	MOVE BITSAV
	TRNN  DTST
IFE USER,<JRST RSWW>
IFN USER,<JRST RSW>
	MOVE 3,TRAJER
	PUSHJ P,SETSET
	MOVE TAC,SAVE.STRING
	JUMPE TAC,FINISH
	MOVEI I,NSCEL-1
TES1:	MOVE AC,DNAME(I)
	CAME AC,TAC
	SOJGE I,TES1
	JUMPL I,[MOVE TAC,[XWD TRANS,TRANS+1]
		BLT TAC,TRANS+=11
		JRST FINISH]
	IMULI I,=12
	ADDI I,DTRANS
	HRLI I,(I)
	HRRI I,TRANS
	BLT I,TRANS+=11
	JRST FINISH

SPACE:	MOVEI 1,=180
	MOVEM 1,COUNT
	TRO TRIP
	LDB 1,[POINT 1,EXE,32]
	MOVEM 1,SOTCHS
	JUMPE 1,.+2
	TRO STCH
IFN DEB,<SETZM ETIM>
	SETZM STRING
	MOVEM BITSAV
IFN TRACK,<SKIPE WALK>
IFN TRACK+USER,<
	JRST[	MOVEM 17,USAVE+17
		MOVEI 17,USAVE
		BLT 17,USAVE+16
		JSR STRADR-1
		HRLZI 17,USAVE
		BLT 17,16
		MOVE 17,USAVE+17
		POPJ P,]
>
IFE USER,<
GETAD:	INIT 17,417
	SIXBIT /AD/
	XWD 0,0
	JRST[	PUSH P,[SIXBIT\A/D WT\]
		PUSHJ P,MSGCHG
		JRST GETAD]
GETXGP:	INIT 14,417
	SIXBIT /XGP/
	XWD 0,0
	JRST .+1
IFE USER+TRACK,<LOCK 1,>
	MOVE 1,[XWD 240001,STRADR]
	CALLI 1,400003
>
	POPJ P,

MSGCHG:	MOVE 1,-1(P)
	CALLI 1,400002
	MOVEI 1,1
	CALLI 1,31
IFE BLUE,<MOVE 1,[SIXBIT/ARM-Y/]>
IFN BLUE,<MOVE 1,[SIXBIT/ARM-B/]>
	CALLI 1,400002
	MOVEI 1,1
	CALLI 1,31
	SUB P,[XWD 2,2]
	JRST @2(P)
UPDL:	FINISH
	BLOCK PDLL

IFN USER+TRACK,<
USAVE:	BLOCK 20
>
IFN TRACK,<WALK:0>

IFN REPORT+TRACK,<
DFILE:	0
	SIXBIT/TMP/
	0
DPPPN:	0
>
LFILE:	0
	SIXBIT/TRJ/
	0
LPPPN:	0

GOODIE:	BLOCK 3
FINISH:	SPCWAR 636367
IFE USER+TRACK,<UNLOCK 1,>
IFE USER,<RELEASE 17,
	RELEASE 14,
>
	SKIPE SEG
	JRST [ATSEG:	MOVE 1,SEG
			CALLI 1,400016
			JRST WAIT
			HLR 1,ETOR		;WHERE SAVE CELL IS TO GO
			MOVEI 2,=11(1)
			HRLI 1,TRANS		;WHERE IT IS
			BLT 1,(2)
			PUSHJ P,SETNAM
			JRST WAIT]
	MOVEM 3,STATUS
	MOVE 1,[XWD TH,THETA]
	BLT 1,THETA+6
	SETZM STRING
	MOVE 1,[XWD TRANS,STRING+1]
	BLT 1,STRING+=12
	MOVE 1,JOB
	MOVEM 1,REPLY
	SKIPN 1
	JRST WAITED
	SEND REPLY
	JRST WAIT
	JRST WAIT

REPLY:	0
	EXE
PROREQ:	
IFE BLUE,<SIXBIT/ARM-B/>
IFN BLUE,<SIXBIT/ARM-Y/>
	EXE
SEG:	0
SOTCH:	0
JOB:	0
IBIT:	000040000000
TICKS:	0
SETFLT:	SKIPN NERVES
	POPJ P,
	MOVEI 6,1
	MOVE 4,NERVES
STL:	MOVE 5,OBS(6)
	MOVEM 5,(4)
	ADDI 4,=16
	SOJGE 6,STL
	POPJ P,

	XLIST
	SUBTTL	SPACEWAR CODE	CONTROL			LOU PAUL
	LIST
;SPACEWAR CODE
IFN USER+TRACK,<0>
STRADR:	MOVE P,[XWD -PDLL,PDL]	;PUSH DOWN STACK
	PUSHJ P,ACREST	;Enters here every 1/60 th. seccond
IFE USER+TRACK,<
	TRNE DTST
	CALLI 400024
IFE BLUE,<DATAO ARM,[2]> IFN BLUE,<CONO ARM,0>
	SOSLE STWT
	CALLI 400024
>
IFE BLUE,<IORI DATWD,2		;YELLOW ARM>
IFN USER+TRACK,<
	TRNE DTST
	JRST SWDONE
>
IFE USER,<
IFN TRACK,<
	SKIPE WALK
	JRST NCHK
>
	CONSZ 40		;CHECK TO SEE IF ON PDP-6
	JRST[	MOVEI TAC,PDPERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	CONI 0,AC
	ANDI AC,7
	CONO 0,2050(AC)
>
	TRZE TRIP		;TURN ON ARM
	JRST[
IFE BLUE,<	DATAO ARM,[3]>
IFN BLUE,<	CONO ARM,1>
NCHK:		
IFE BLUE,<	MOVEI K,51B23		;-10 VOLT REFERENCE>
IFN BLUE,<	MOVEI K,50B23		;-10 VOLT REFERENCE>
		PUSHJ P,PREAD		;REFTR← -10 volt reference
		LDB AC,SNUM		;PICK UP MIDDLE SAMPLE
		ANDI DACVAL,7777	;LEAVE LAST SAMPLE IN J
		ADDI AC,(DACVAL)	;ADD THEM TOGETHER
		CAIGE AC,17000		;IS THE FUSE BLOWN
		JRST[	MOVEI TAC,REFERR
			MOVEM TAC,TRAJER
			JRST TOFF]
		MOVEM AC,REFTR		;REFERENCE READING
		MOVEI I,5		;FIRST THREE TACHS
ZVEL:		MOVE K,VCHAN(I)
		JUMPE K,ZVELE
		PUSHJ P,PREAD
		LDB AC,SNUM
		ANDI DACVAL,7777
		ADDI AC,(DACVAL)
		MOVEM AC,VZ(I)
IFN TACH,<	SETZM VDTH(I)>
ZVELE:		SOJGE I,ZVEL
		SETZM MOVT
		JRST .+1]
IFN TRACK,<
	SKIPE WALK
	JRST TOK
>
	MOVEI AC,=16666
	MOVEM AC,ETIME
	MOVSI AC,(1.0)
	MOVEM AC,TDF
IFE USER,<
	CONI CLOCK,AC
	TLZ AC,777774
	JRST STIME		;TIME IN MICRO SECONDS
>
IFN USER,<JRST .+2>
NXTJIF:
IFN USER,<JRST SWDONE>
IFN DEB<
	MOVEM 17,ACSAV+17
	MOVEI 17,ACSAV
	BLT 17,ACSAV+16
	MOVE 17,ACSAV+17
>
IFN TRACK,<
	SKIPE WALK
	JRST TOK
>
IFN REPORT+TRACK,<
	HRRZI TAC,(DATA)
	CAIL TAC,DATO+DBL-1
	JRST[	SUB DATA,[XWD DBL,DBL]
		MOVE TAC,[XWD DATO+DBL,DATO]
		BLT TAC,(DATA)
		HRRZI AC,(DATA)
		HRRZ TAC,DATIWD
		SUBI AC,(TAC)
		JRST RDP]
	HRRZI AC,(DATA)
	HRRZ TAC,DATIWD
	SUBI AC,(TAC)
	HRRZ MQ,DATPT
	SUBI TAC,(MQ)
	IMUL AC,TAC
RDP:	SKIPLE AC
IFN TRACK,<PUSH DATA,[0]>
IFN REPORT,<PUSH DATA,[SIXBIT/ERROR/]>
	MOVEI AC,DATO-1
	HRRZ TAC,DATIWD
 	CAIL TAC,DATO+DBL-1
	HRRM AC,DATIWD
	MOVEM DATA,DATPT
>
IFE USER,<
	MOVEI TAC,=2000
WATSTL:
	CONI CLOCK,AC
	TLZ AC,777774
	SUB AC,LTIME
	JUMPGE AC,.+2
	ADD AC,[=1000000]
IFN REPORT,<
	CAIN TAC,=2000
	JRST[	PUSH DATA,[<SIXBIT/TICK/>+1]
		PUSH DATA,AC
		JRST .+1]
>
	MOVEM AC,ETIME
	CAIGE AC,=16667		;delta T greater than 16.7 msec.?
	SOJGE TAC,WATSTL
DWAIT:	JUMPL TAC,TE
	MOVE MQ,AC
	FSC MQ,233
	FMPR MQ,[0.00006]
	MOVEM MQ,TDF
	MOVE AC,LTIME
	ADD AC,ETIME
	TLNE AC,777774
	SUB AC,[=1000000]
STIME:	MOVEM AC,LTIME		;LTIME←LTIME+16.7 msec.
>
	SOSGE COUNT
	JRST[TE:MOVEI AC,TERR
		MOVEM AC,TRAJER
		JRST ZITO]
IFN ADJUST,<
	MOVEI K,14B23
	MOVEM BITSAV
	MOVEI I,2
RNP:	PUSHJ P,PREAD
	LDB AC,SNUM
	FSC AC,221
	JSA 16,EXP
	JUMP AC
	CAMG [1.2]
	SETZ 0,
	FMPR [0.20]
	MOVEM FACTOR(I)
	SOJGE I,RNP
	MOVE BITSAV
	MOVE I,ADJI
	JUMPL I,MAIN
IFN ADJG,<
	MOVE AC,ADJV
	FMPR AC,FACTOR
	MOVEM AC,KV(I)
	MOVE AC,ADJE
	FMPR AC,FACTOR+1
	MOVEM AC,KE(I)		;POT 13
	MOVE AC,ADJIF
	FMPR AC,FACTOR+2
	MOVEM AC,KI(I)	;POT 14
>
>
MAIN:	SKIPE MOVT		;NO MOVING RESTORE
	JRST[	MOVE AC,ETIME
		ADDB AC,MOVTIM
		ASH AC,-16
		FSC AC,233
		MOVEI I,2
		MOVE K,MOVT
	MOV4:	MOVE TAC,MOVV(I)
		FMPR TAC,AC
		MOVEM TAC,13(K)
		SUBI K,4
		SOJGE I,MOV4
		JRST .+1]
	TRNE RUN
	JRST[	PUSHJ P,EVAL
		PUSHJ P,NXTH
		JRST .+1]
	MOVEI J,6
	PUSHJ P,ANGLES		;read joint angles
	TRNE RONLY		;read angles only?
	JRST[	PUSHJ P,HEAD	;read hand also
		MOVE AC,HAND
		MOVEM AC,SET
		PUSHJ P,REFLEX
		PUSHJ P,READED]	;DOES NOT RETURN
	TRNN RUN
	JRST[	SKIPG TAC,FUNCT	;not run, mabye a function
		PUSHJ P,SETSET
		JUMPL TAC,NEXT
		JUMPE TAC,[CAIL CBUF,BUF;try next buffer
			SETOM (CBUF)	;dont do this if first buffer
			MOVEM CBUF,CBUFS
			ADDI CBUF,1001
			CAILE CBUF,BUF2
			MOVEI CBUF,BUF		;Advance buffer
			MOVE 2,(CBUF)
			JUMPL 2,[BUFWT:MOVE CBUF,CBUFS;wait for buffer
				JRST SWDONE]
			JUMPN 2,BUFRDY		;the last buffer stop.
			JRST[	TRO DTST
				MOVEI 2,WAITIN
				HRRM 2,JUMPBK
				MOVEM 2,STKPTR
		IFN REPORT,<PUSH DATA,[0]>
				JRST BUFWT]
		BUFRDY:	HRRZ 2,(CBUF)
			ADDI 2,-1(CBUF)
			HRRZM 2,STKPTR
			JRST NEXT]
		CAIN TAC,6
		JRST SETCH
		JRST TT]	;otherwise perform function
	JUMPE J,[RUDONE:
		PUSHJ P,SETSET
		TRZ DROP+RUN+FINAL+WOB+HOMEF	;if J is zero then end of run
		TDZ DATWD,[770001252500]
IFN BLUE,<	SETZM MOTDAT
		MOVE AC,MOTDAT+1
		TLZ AC,774000
		MOVEM AC,MOTDAT+1
		MOVE AC,MOTBRK
		TLZ AC,770000
		MOVEM AC,MOTBRK
		CONO ARM,(AC)
		DATAO ARM,MOTDAT
		DATAO ARM,MOTDAT+1
>
		TRZE STP
		JRST[	MOVEI TAC,STPERR
			MOVEM TAC,TRAJER
			POP P,TAC
			JRST TOFF]
		SKIPN AC,FUNCT
		JRST NEXT
		CAIN AC,22
		JRST[	TRZ HCL
IFE BLUE,<
			SETZ AC,
			DPB AC,[POINT 7,TDAT,20]
			DATAO DEVT,TDAT
>
IFN BLUE,<
			MOVEI I,6
			PUSHJ P,MOTSTP
>
			SETZM FUNCT
			JRST NEXT]
		MOVEI AC,=16667
		MOVEM AC,ETIME
		JRST .+1]
TT:	PUSHJ P,REFLEX
DISP:	MOVE AC,FUNCT
	JUMPL AC,.+2
	CAILE AC,MAXFN
	JRST[	MOVEI TAC,FUNERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	JRST @.+1(AC)		;Perform function
	NXTJIF	;0
	HOPEN	;1
	HCLOSE	;2
	NXTJIF	;3
	PLACE	;4
	NUDGE	;5
	SETCH	;6
	STOP	;7
	SAVE	;10
	RESTORE	;11
	CENTER	;12
	SET.ARM	;13 SET_ARM
	WOBBLE	;14
	SEA	;15
	AOJI	;16
	IKU	;17 TO	
	HOME	;20
	PATH	;21
	TOOL	;22
	DRIVE	;23
	MOVING	;24
	ASSIGN	;25
	SOJGI	;26
	PROCEED	;27
	PAUSE	;30
	RETURN	;31
	VOPEN	;32
	VCLOSE	;33

	XLIST
	SUBTTL	SPACEWAR CODE	ANGLE MEASUREMENT	LOU PAUL
	LIST
;MEASURE JOINT ANGLE
ANGLES:	MOVEI I,5
	JFCL 10,.+1
	MOVEI L,10
RSTRT:	MOVE K,CHAN(I)
	SOJL L,REDER
IFE USER,<
	PUSHJ P,PREAD
	LDB AC,SNUM
	MOVEM AC,POT(I)
	ANDI DACVAL,7777
	ADDI AC,(DACVAL)
	SETZ TAC,
	DIV AC,REFTR
	ASH AC,-26
	MOVE TAC1,AC
	TRNE POTREAD
	JRST[	ADDI TAC1,=1000
		JRST EXON]
	ASHC AC,-11		;LEAVE INDEX INTO NON-LINEAR DIFF TABLE IN AC
	LDB MQ,NON(I)		;GET LOWER DIFF
	ADDI TAC1,(MQ)		;ADD IT TO READING
	AOJ AC,
	LDB AC,NON(I)		;GET UPPER DIFFERENCE
	SUBI AC,(MQ)
	MUL TAC,AC		;INTERPOLATE
	ADD TAC1,TAC		;CORRECTED READING
	FSC TAC1,226		;FLOAT
	FMPR TAC1,SCALE(I)
IFN CALIB,<
	SKIPL SETOFF(I)
	JRST NOTT
	MOVE TAC,TH(I)
	FSBR TAC,TAC1
	MOVEM TAC,OFF(I)
	HLRZ AC,CHAND(I)
	MOVEM TAC,(AC)
	HLLM AC,SETOFF(I)	;SET LH OF SET FLAG TO ZERO
NOTT:
>
	FADR TAC1,OFF(I)
>
IFN USER,<MOVE TAC1,TH(I)>
EXON:
	MOVEM TAC1,TH(I)		;THETA
THS:
IFN REPORT,<
	MOVE MQ,TH(I)
	FSBR MQ,T0(I)
SPOS:	MOVEM MQ,JER(I)
>
;MEASURE JOINT VELOCITY
	MOVE TAC1,TH(I)
	EXCH TAC1,THP(I)
	MOVE K,VCHAN(I)
IFE USER,<JUMPN K,.+2>
	JRST[DIFPOS:
		FSBR TAC1,THP(I)
		FDVR TAC1,TDF
		MOVNM TAC1,TD(I)
		JRST SERVO]
NVEL:	PUSHJ P,PREAD
	LDB AC,SNUM
	ANDI DACVAL,7777
	ADDI AC,(DACVAL)
IFN SMV,<
	CAML AC,MAXVEL(I)
	MOVEM AC,MAXVEL(I)
	CAMG AC,MINVEL(I)
	MOVEM AC,MINVEL(I)
>
	CAIG AC,17000
	CAIG AC,776
	JRST DIFPOS
	SUB AC,VZ(I)
	FSC AC,216
IFN TACH,<
	FMPR AC,TDF
	FADRM AC,VDTH(I)
	JRST DIFPOS
>
	FMPR AC,VSCALE(I)
	MOVEM AC,TD(I)
	XLIST
			SUBTTL			SPACEWAR CODE			SERVO CALC.			LOU PAUL
	LIST
SERVO:	TRNN RUN
	JRST[	JSR WIPER
		SOJGE I,RSTRT
		POPJ P,]
SERVL:	MOVE TAC,TH(I)
	FSBR TAC,T0(I)		;POSITION ERROR
	MOVEM TAC,ET0(I)
	TRNE FINAL
	JRST NULL		;MODIFY THETA BY INTEGRAL TERM
	MOVE TAC1,T0(I)
	FSBR TAC1,T0P(I)
	FDVR TAC1,TDF
	MOVE AC,TAC1
	FSBR AC,TDP(I)
	MOVEM TAC1,TDP(I)
	FDVR AC,TDF
	MOVEM AC,TDD(I)
	FMPR AC,CII(I)
MODT0:	HRRZ K,BMASK(I)
	TDNN K,FBI
	JRST [	PDIR:SETZM ET0(I)
		FMPR AC,CII(I)
		JRST NOBACK]
	TRNE FINAL		;ONLY INTEGRATE WHEN FINAL IS ON
	FADRM TAC,ERRINT(I)
IFN REPORT,<JRST .+1>
	MOVN TAC,TD(I)
	FADR TAC,TAC1
IFN REPORT,<SVEL:MOVNM TAC,VER(I)>
	FMPR TAC,KV(I)		;VELOCITY GAIN
	FADR AC,TAC
	FMPR AC,CII(I)
	MOVN TAC,ET0(I)
	FMPR TAC,KE(I)		;ERROR GAIN
	FADR AC,TAC
	MOVN TAC,ERRINT(I)
	FMPR TAC,KI(I)
	FDVR TAC,CII(I)
	FADR AC,TAC
NOBACK:	CAIE I,2
	FMPR AC,[IRAD:0.0174532925]	;JOINT 3 IS IN INCHES
	MOVEM AC,TORE(I)
	FADR AC,CI(I)		;GRAVITY TERM
	MOVEM AC,T(I)			;ERROR CORRECTION TORQUE
	XLIST
			SUBTTL			SPACEWAR CODE			DRIVE CALC.			LOU PAUL
	LIST
;DRIVE THE ARM
LS0:	TDNN DATWD,BMASK(I)
	JRST ISP
	MOVE MQ,TD(I)
	JUMPN MQ,.+2
	MOVN MQ,ET0(I)
	FMPR MQ,T(I)
	JUMPL MQ,[	MOVN MQ,V0(I)
			JRST IVWD]
	SKIPN T(I)
	JRST[		SKIPN TD(I)
			JRST CEMF
			MOVE MQ,V0(I)
			SKIPGE TD(I)
			MOVN MQ,MQ
			JRST CEMF]
	JUMPE MQ,NOEX
	MOVE MQ,V0(I)
IVWD:	SKIPGE T(I)
	MOVN MQ,MQ
NOEX:	MOVE AC,KM(I)
	FMPR AC,T(I)
	FADR MQ,AC
CEMF:
IFE BLUE,<
	MOVE AC,EMF(I)
	FMPR AC,TD(I)
	MOVSI TAC1,(30.0)
	JUMPGE MQ,DRVLT
	MOVN AC,AC
DRVLT:	FSBR TAC1,AC			;Available drive voltage
	CAMGE TAC1,[1.0]
	JRST MAXDRV
VELOK:	MOVM AC,MQ
	FDVR AC,TAC1		;AC= REL TIME ON
	FIX AC,211000		;1=16
	CAML AC,[4000000]
	JRST DHO		;too much force stop the arm
	CAILE AC,776000
MAXDRV:	MOVEI AC,776000
IFN REPORT+TRACK+USER+DEB,<
	MOVEM AC,MOTOR(I)
	SKIPGE MQ
	MOVNS MOTOR(I)
>
	TRC AC,400000
	HRL AC,I
SETDRV:
IFN TRACK,<SKIPN WALK>
	DATAO WIDTH,AC
	TDZ DATWD,DMASK(I)
	JUMPGE MQ,ISP
	TDO DATWD,DMASK(I)
ISP:	SKIPE REV(I)
	TDC DATWD,DMASK(I)
	DATAO ARM,DATWD
	JSR WIPER
	SOJGE I,RSTRT
>
IFN BLUE,<
	FIX MQ,233000		;1=16
IFN REPORT+TRACK+USER+DEB,<
	MOVEM MQ,MOTOR(I)
>
	MOVM AC,MQ
	CAIL AC,200
	JRST DHO		;too much force stop the arm
	SKIPE REV(I)
	MOVN MQ,MQ
	PUSH P,MQ
	PUSHJ P,MOTDRV
ISP:	JSR WIPER
	SOJGE I,RSTRT
>
	JFCL 10,DHO
IFN TRACK,<SKIPN WALK>
;STOP ON FORCE
IFN SIMU,<			;IF IN SIMU PREDICT NEXT VALUE OF THETA
	MOVEI I,5
PNT:	MOVE AC,T0(I)		;PRESENT VALUE
	FSC AC,1		;X 2
	FSBR AC,T0P(I)		;PREVIOUS VALUE
	MOVEM AC,TH(I)		;IS NEXT VALUE
	SOJGE I,PNT
>
IFN REPORT,<			;OUTPUT ERRORS AND DRIVES
	HRLI AC,PBLK
	HRRI AC,1(DATA)
	ADD DATA,[XWD 25,25]
	BLT AC,(DATA)
>
	SETZM ASTP
	TRNE STP
	JRST[	MOVEI I,5
	MTL:	MOVE MQ,TORE(I)
		FDVR MQ,F0(I)
		HRRZ K,BMASK(I)
		TDNN K,FBI
		MOVE MQ,STQ(I)
		FMPR MQ,STQ(I)
		FADRM MQ,ASTP
		SOJGE I,MTL
		MOVE AC,ASTP
		CAML AC,SDTQ
		JRST[	AOS AC,ASTPP
			CAIL AC,2
			JRST[	TRZ STP
				SUB P,[XWD 1,1]
				JRST RUDONE]
			POPJ P,]
		SETZM ASTPP
		POPJ P,]
	POPJ P,

NULL:	SETZB AC,TAC1
	TLNN DATWD,NNUL
	TRNE WOB
	SOJA J,MODT0
	TDNN DATWD,GMASK(I)
	SOJA J,LS0
	MOVE MQ,FBI
	TDNN MQ,GMASK(I)
	SOJA J,PDIR
	MOVM MQ,TAC			;|POSITION ERROR|
	CAMLE MQ,ERR(I)			;COMPARE POSITION ERROR
	JRST MODT0
	MOVM MQ,TD(I)
	CAMLE MQ,ERR(I)
	JRST MODT0
SO:	TDZ DATWD,BMASK(I)
IFN REPORT+TRACK+USER+DEB,<SETZM MOTOR(I)>
IFE BLUE,<
	DATAO ARM,DATWD
	HRLZ MQ,I
	TRC MQ,400000
	DATAO WIDTH,MQ
>
IFN BLUE,<PUSHJ P,MOTSTP>
	SOJA J,LS0
	POPJ P,

	XLIST
	SUBTTL	SPACEWAR CODE				LOU PAUL
	LIST
WIPER:	0
TNW:	SKIPN TAC,CHAND(I)	;IF NON ZERO THEN XWD OFF,CHAN
	JRST 2,@WIPER
	MOVE AC,POT(I)		;OCTAL POT READING
	CAIGE AC,1400
	JRST[	AOBJN TAC,.
	SND:	SKIPN MQ,(TAC)
		JRST 2,@WIPER
		MOVEM MQ,CHAN(I);NEW CHANNEL
		MOVEM TAC,CHAND(I)
		HLR TAC,TAC
		MOVE MQ,(TAC)
		MOVEM MQ,OFF(I)
IFN CALIB,<
		SKIPE SETOFF(I)
		SETOM SETOFF(I)
>
		JRST 2,@WIPER]
	CAILE AC,6400
	JRST[	SUB TAC,[XWD 1,1]
		JRST SND]
	JRST 2,@WIPER
IFN CALIB,<
SETOFF:	0
	0
	0
	0
	0
	0
>

NXTH:	MOVE AC,[XWD T0,T0P]
	BLT AC,T0P+5
	TRNE WOB
	JRST[	MOVEI I,2
	WL1:	SOSGE J,WOBCNT(I)
		JRST[	MOVEI J,=19
			MOVEM J,WOBCNT(I)
			JRST WL2]
	WL2:	MOVE AC,SIN(J)
		FMPR AC,WOBMAG
		FADRM AC,TFF+3(I)
		SOJGE I,WL1
		JRST .+1]
	TRNE DROP
	JRST[	MOVEI I,5
	OL:	MOVE AC,DELTH(I)
		FMPR AC,TDF
		FADRM AC,DTH(I)
		SOJGE I,OL
		JRST JALS]
NTH:
	TRNE INCREM
	JRST[	MOVE MQ,ETIME
		ADDB MQ,TICKS
		CAML MQ,NTICKS
		JRST[	TRNE HOMEF
			JRST[	
				SOSGE TAC,HOMEC
				JRST FINC
				SUB MQ,NTICKS
				MOVEM MQ,TICKS
				MOVEI I,5
			NXUD:	MOVE AC,DELTH(I)
				FADRM AC,DTH(I)
				SOJGE I,NXUD
				JUMPN TAC,[
					MOVEI I,5
				NXMS:	MOVE AC,THM(I)
					MOVEM AC,DELTH(I)
					SOJGE I,NXMS
					MOVE AC,TIMM
					JRST NXSS]
				MOVEI I,5
			NXFS:	MOVE AC,THF(I)
				MOVEM AC,DELTH(I)
				SOJGE I,NXFS
				MOVE AC,TIMF
			NXSS:	MOVEM AC,NTICKS
				ASH AC,-15
				ADDI AC,=60
				MOVEM AC,COUNT
				MOVEI AC,7
				ADDB AC,HDP
				PUSHJ P,UPDC
				MOVE MQ,TICKS
				JRST NXIN]
		FINC:	TRZ INCREM
			SKIPN TP			;ARE WE IN A TRAJECTORY
			TRO FINAL			;NO START INTEGRATING
			MOVEI AC,=600
			MOVEM AC,COUNT
			SKIPN TP(I)
			TLNN DATWD,INT
			JRST FIL1
			MOVE AC,[XWD NC,CI]
			BLT AC,CII+5
		FIL1:	MOVEI I,5
		FIL:	MOVE AC,DELTH(I)
			FADRB AC,DTH(I)
			FADR AC,TFF(I)
			MOVEM AC,T0(I)
			SETZM DELTH(I)
			SOJGE I,FIL
			JRST TFSTP]
	NXIN:	SETZ I,
		DIV MQ,NTICKS
		ASH MQ,-10
		FSC MQ,200
		MOVSI AC,(6.0)
		FMPR AC,MQ
		FADR AC,[-15.0]
		FMPR AC,MQ
		FADR AC,[10.0]
		FMPR AC,MQ
		FMPR MQ,MQ
		FMPR MQ,AC
		MOVEI I,5
	LDC:	MOVE AC,DELTH(I)
		FMPR AC,MQ
		FADR AC,DTH(I)
		FADR AC,TFF(I)
		MOVEM AC,T0(I)
		SKIPN TP
		TLNN DATWD,INT
		JRST LDC1
		MOVE AC,DCI(I)
		FMPR AC,MQ
		FADR AC,PC(I)
		MOVEM AC,CI(I)
		MOVE AC,DCII(I)
		FMPR AC,MQ
		FADR AC,PCC(I)
		MOVEM AC,CII(I)
	LDC1:	SOJGE I,LDC
		JRST TFSTP]
JALS:	MOVEI I,5
JAL:	MOVE AC,DTH(I)
	FADR AC,TFF(I)
	MOVEM AC,T0(I)
	SOJGE I,JAL
;CHECK THE SET POINT LIMIT
TFSTP:	MOVEI I,5
TSTP:	MOVE AC,T0(I)
	CAMG AC,USTOP(I)
	CAMGE AC,LSTOP(I)
	JRST[	MOVE TAC,AC
		CAMLE AC,USTOP(I)
		JRST[	FSBR AC,T0P(I)
			JUMPL AC,.+1
                      	FSBR TAC,USTOP(I);	X-L
			MOVE AC,TAC
			FADR AC,LDEL(I);	1+X-L
			FDVR TAC,AC
			FMPR TAC,LDEL(I)
			MOVE AC,USTOP(I)
			FADR AC,TAC
			MOVEM AC,T0(I)
			JRST .+1]
		FSBR TAC,LSTOP(I);	X-L
		MOVE AC,TAC
		FSBR AC,LDEL(I);	1-X-L
		FDVR TAC,AC
		FMPR TAC,LDEL(I)
		MOVE AC,LSTOP(I)
		FSBR AC,TAC
		MOVEM AC,T0(I)
		JRST .+1]
	SOJGE I,TSTP
	POPJ P,
EVAL:	MOVEI I,5
EL:	MOVE J,TP(I)	;POINTS TO KOE5
	JUMPE J,NE
	MOVE MQ,ETIME
	ADDB MQ,TJ(I)
	CAML MQ,TN(I)	;NUMBER OF TICKS THIS SEGMENT
	JRST[			;END OF SEGMENT
		CAIN I,5
		JRST[			;JOINT 6 HAS CONTROL WORD
			MOVE TAC,TICKS	;TURN ON INCREM ONLY IN SECOND SEGMENT
			CAMGE TAC,NTICKS
			TRO INCREM
			LDB TAC,[POINT 3,1(J),2]	;LEVEL
			JUMPN TAC,[			;SWITCH POINT
				SKIPG LOOP(TAC)
				JRST[			;INITIALIZE LOOP
					LDB AC,[POINT 15,1(J),17]
					MOVEM AC,LOOP(TAC)	;COUNT
					JRST SETALT]
				SOSLE LOOP(TAC)		;IN LOOP
			SETALT:	TRO ALT
				JRST GETNXT]
			JRST GETNXT]
	GETNXT:	TRNE ALT
		JRST[	LDB J,[POINT 9,(J),8]	;ALT POINTER
			JRST GOT]
		LDB J,[POINT 9,(J),17]		;NEXT POINTER
		JUMPE J,[SETZM TP(I)
			TRO FINAL
			MOVEI AC,=600
			MOVEM AC,COUNT
			MOVE TAC,[XWD NC,CI]
			BLT TAC,CI+=11
			MOVE AC,A4(I)
			ADD AC,A3(I)
			ADD AC,A2(I)
			ADD AC,A1(I)
			ADD AC,A0(I)
			XOR AC,SC(I)
			FSC AC,0
			MOVEM AC,TFF(I)
			JRST NE]
	GOT:	ADDI J,(CBUF)
		MOVEM J,TP(I)
		SUB MQ,TN(I)	;MICRO SEC INTO NEW SEGMENT
		MOVEM MQ,TJ(I)	;INITIALIZE
		HRRZ AC,(J)
		ASH AC,16
		MOVEM AC,TN(I)
		CAIN I,5
		JRST[
			HRRZ AC,1(J)
			ADDI AC,(CBUF)
			MOVEI MQ,6(AC)
			MOVEI K,5
			MOVE MQ,(MQ)		;NEW DATWD
		FTF:	MOVE TAC,GMASK(K)
			TDNN TAC,FBI
			JRST[	TDNN TAC,MQ	;WAS FREE
				JRST FNF	;STILL FREE
				MOVE TAC,TH(K)
				MOVEM TAC,T0(K)
				FSBR TAC,TFF(K)
				MOVEM TAC,DTH(K)
				MOVNM TAC,DELTH(K)
				MOVE TAC,TN+5
				ASH TAC,16
				MOVEM TAC,NTICKS
				SETZM TICKS
				TRO INCREM
				JRST FNF]
			TDNN TAC,MQ		;NOT FREE
			JRST[	MOVE TAC,TDD(K)	;NOW IS FREE
				CAIE K,2
				FMPR TAC,IRAD
				FMPR TAC,CII(K)
				FMPR TAC,CII(K)
				FSBR TAC,T(K)
				MOVNM TAC,NC(K)
				JRST FNF]
		FNF:	SOJGE K,FTF
			PUSHJ P,UPDC
			MOVEI I,5		;RESTORE I
			JRST DDELC]
	DDELC:	HLRE AC,-3(J)
		MOVEM AC,A4(I)
		HRRE AC,-3(J)
		MOVEM AC,A3(I)
		HLRE AC,-2(J)
		MOVEM AC,A2(I)
		HRRE AC,-2(J)
		MOVEM AC,A1(I)
		HLLZ AC,-1(J)
		MOVEM AC,SC(I)
		HRRE AC,-1(J)
		MOVEM AC,A0(I)
		MOVE MQ,TJ(I)
		JRST ELL]
ELL:	DIV MQ,TN(I)
	TLNE DATWD,INT
	CAIE I,5
	JRST ELL1
	MOVEI K,=11
	HLRE TAC,MQ
	FSC TAC,211
DECC:	MOVE AC,DCI(K)
	FMPR AC,TAC
	FADR AC,PC(K)
	MOVEM AC,CI(K)
	SOJGE K,DECC
ELL1:	MOVE AC,A4(I)
	MUL AC,MQ
	ADD AC,A3(I)
	MUL AC,MQ
	ADD AC,A2(I)
	MUL AC,MQ
	ADD AC,A1(I)
	MUL AC,MQ
	ADD AC,A0(I)
	XOR AC,SC(I)
	FSC AC,0
	MOVEM AC,TFF(I)
NE:	SOJGE I,EL
	TRZ ALT
	POPJ P,

DHO:	AOJ I,			;DISASTER HAS OCCURED
	LSH I,3			;JOINT NUMBER
	IORI I,EXER
	MOVEM I,TRAJER
	POP P,I
ZITO:	SETZM ERRINT
	MOVE AC,[XWD ERRINT,ERRINT+1]
	BLT AC,ERRINT+5

TOFF:	MOVE AC,STKPTR
	MOVEM AC,ERRSTK
	DATAO DEVT,[0]
	MOVEI L,1000
	TRNE RONLY
	JRST READED
	TRZ RUN+FINAL+DROP+INCREM+HCL+STP+WOB+HOMEF	;STOP THE ARM
	SETZM TP
	MOVE AC,[XWD TP,TP+1]
	BLT AC,TP+5
	PUSHJ P,SETSET
MOVNXT:	AOS AC,STKPTR
MOVNX:	SOJL L,[ LE:
		MOVEI TAC,LOOPER
		MOVEM TAC,TRAJER
		JRST PW]
	HLRE TAC,@STKPTR
	CAIN TAC,3		;IS IT A WAIT
	JRST[	CAIL AC,BUF	;ALREADY IN A WAIT STATE
		JRST PW
		JRST MOVNXT]
	CAIN TAC,SKPERR
	JRST[	HRRZ TAC,(AC)
		CAMN TAC,TRAJER
		JRST[	AOS STKPTR
			JRST NEXT]
		JRST MOVNXT]
	CAIN TAC,SKPNER
	JRST[	HRRZ TAC,(AC)
		CAME TAC,TRAJER
		JRST[	AOS STKPTR
			JRST MOVNXT]
		JRST NEXT]
	CAIN TAC,SKPSER
	JRST[	LDB MQ,[POINT 3,(AC),35]
		LDB TAC,[POINT 3,TRAJER,35]
		CAIN TAC,(MQ)
		JRST[	HRRZ TAC,(AC)
			ANDI TAC,777770
			TDNE TAC,TRAJER
			JRST[	AOS STKPTR
				JRST NEXT]
			JRST MOVNXT]
		JRST MOVNXT]
	CAIN TAC,JMPREL
	JRST[	HRRE TAC,(AC)
		ADDI AC,(TAC)
		HRRM AC,STKPTR
		JRST MOVNX]
PW:	CAIL AC,BUF
	HRRM AC,JUMPBK
	MOVEI AC,WAITIN
	MOVEM AC,STKPTR
	MOVEI AC,3
	MOVEM AC,FUNCT
IFN TRACK,<SKIPN WALK>
IFE BLUE,<DATAO ARM,[2]		;STOP THE ARM>
IFN BLUE,<CONO ARM,0
	SETZM MOTBRK
	SETZB AC,MOTDAT
	DATAO ARM,AC
	MOVEI AC,1
	MOVEM AC,MOTDAT+1
	DATAO ARM,AC
>
IFN REPORT,<PUSH DATA,[0]>
READED:	TRO DTST
IFN DEB,<
	MOVEM 17,ACSAV+17
	MOVEI 17,ACSAV
	BLT 17,ACSAV+16
>
	JRST SWDONE

NEXT:	TRNE RUN
	JRST[	SETZM FUNCT
		JRST NXTJIF]
	MOVEI L,1000
	MOVE AC,FUNCT
	CAIE AC,6		;SET TOUCH
	JRST[	JUMPE AC,.+1
		MOVE TAC,SOTCHS
		MOVEM TAC,SOTCH
		TRZ STCH
		JUMPE TAC,.+1
		TRO STCH
		JRST .+1]
IFN TRACK,<SKIPN WALK>
IFE BLUE,<
	MOVEI DATWD,2
	DATAO ARM,DATWD		;STOP THE ARM
>
IFN BLUE,<CONO ARM,0
	SETZM MOTBRK
	SETZB AC,MOTDAT
	DATAO ARM,AC
	MOVEI AC,1
	MOVEM AC,MOTDAT+1
	DATAO ARM,AC
>
INCPIC:	AOS AC,STKPTR
PICKUP:	SOJL L,LE
	HLRE TAC,@STKPTR
	CAIN TAC,JMPABS
	JRST[JMPA:HRRZ TAC,@STKPTR
		MOVEM TAC,STKPTR
		JRST PICKUP]
	CAIN TAC,SKPERR
	JRST[	HRRZ TAC,@STKPTR
		CAMN TAC,TRAJER
		AOS STKPTR
		JRST INCPIC]
	CAIN TAC,SKPNER
	JRST[	HRRZ TAC,@STKPTR
		CAME TAC,TRAJER
		AOS STKPTR
		JRST INCPIC]
	CAIN TAC,SKPSER
	JRST[	LDB MQ,[POINT 3,@STKPTR,35]
		LDB TAC,[POINT 3,TRAJER,35]
		CAIN TAC,(MQ)
		JRST[	HRRZ TAC,@STKPTR
			ANDI TAC,777770
			TDNE TAC,TRAJER
			AOS STKPTR
			JRST INCPIC]
		JRST INCPIC]
	CAIN TAC,JMPREL
	JRST[	HRRE TAC,@STKPTR
		ADD TAC,STKPTR
		HRRM TAC,STKPTR
		JRST PICKUP]
	SETZM TRAJER
	MOVEI I,1000
	MOVEM I,COUNT
	TRZE TAC,MERGE
	TRO BOTH
	MOVEM TAC,FUNCT
	CAIN TAC,3		;IS IT A WAIT
	JRST[	MOVE TAC,STKPTR
		CAIG TAC,BUF
		JRST MODSTK
		AOJ TAC,
		HRRM TAC,JUMPBK
		HRRE TAC,@STKPTR
		ADDI TAC,(CBUF)
		SKIPE AC,(TAC)
		JRST[	AOJ TAC,
			MOVEI AC,STRING-1(AC)
			HRLI TAC,(TAC)
			HRRI TAC,STRING
			BLT TAC,(AC)
			JRST MODSTK]
	MODSTK:	MOVEI TAC,WAITIN
		MOVEM TAC,STKPTR
IFN REPORT,<PUSH DATA,[0]>
		TRO DTST
		PUSHJ P,SWDONE]
MARK:
IFN REPORT,<
	PUSH DATA,[<SIXBIT/NEXT/>+1]
	MOVE TAC,FUNCT
	HRL TAC,0
	PUSH DATA,TAC
>

;	JRST DISP
SWDONE:	PUSHJ P,ACSAVE
IFE USER,<
IFN TRACK,<
	SKIPE WALK
	JRST @STRADR-1
>
	CALLI 400024
>
IFN USER,<
	JRST @STRADR-1
>

SETSET:	MOVEI I,5
TCHK:	MOVE J,TP(I)
	JUMPE J,ATEND
	SETZM TJ(I)
	SETZM TN(I)
GNN:	LDB J,[POINT 9,(J),17]
	ADDI J,(CBUF)
	MOVEM J,TP(I)
	CAIN J,(CBUF)
	JRST ATEND
	JRST GNN
ATEND:	SOJGE I,TCHK
	PUSHJ P,EVAL
	MOVEI I,5
FDTH:	MOVE AC,TH(I)
	MOVEM AC,THP(I)
	MOVEM AC,T0(I)
	MOVEM AC,T0P(I)
	FSBR AC,TFF(I)
	MOVEM AC,DTH(I)
	SETZM DELTH(I)
	SETZM TD(I)
	SETZM TDP(I)
	SETZM TDD(I)
	SOJGE I,FDTH
	SETZM NTICKS
	TRZ WOB+INCREM+DROP+FINAL+RUN+ALT+HOMEF
	POPJ P,


UPDC:	MOVE TAC,[XWD NC,PC]
	BLT TAC,PCC+5
	HRL AC,AC
	HRRI AC,NC
	BLT AC,NC+6
	MOVE DATWD,NC+6
	TLNE DATWD,NXTFN
	JRST[	AOS AC,STKPTR
		HLRE AC,(AC)
		MOVEM AC,FUNCT
		JRST .+1]
	HRRZM DATWD,FBI
	MOVEI I,5
UPL:	MOVE AC,NC(I)
	HLLZM AC,NC(I)
	HRLZM AC,NCC(I)
	MOVE AC,MOTARM(I)
	FADRB AC,NCC(I)
	TLNN DATWD,INT
	JRST UPL1
	FSBR AC,PCC(I)
	MOVEM AC,DCII(I)
	MOVE AC,NC(I)
	FSBR AC,PC(I)
	MOVEM AC,DCI(I)
UPL1:	TDNE DATWD,BMASK(I)
IFE BLUE,<TDO DATWD,BMASK(I)>
IFN BLUE,<PUSHJ P,MOTSTA>
	SOJGE I,UPL
	TLNE DATWD,INT
	POPJ P,
	MOVE AC,[XWD NC,CI]
	BLT AC,CII+5
	POPJ P,
	
ACSAVE:	MOVEM BITSAV
	MOVEM CBUF,CBUFS
	MOVEM DATWD,DATWDS
IFN REPORT+TRACK,<MOVEM DATA,DATPT>
	POPJ P,

ACREST:	MOVE BITSAV
	MOVE CBUF,CBUFS
	MOVE DATWD,DATWDS
IFN REPORT+TRACK,<MOVE DATA,DATPT>
	POPJ P,
;MOTOR DRIVE FUNCTIONS
IFN BLUE,<
MOTDRV:	MOVE MQ,-1(P)		;JOINT IN I
	CAIL MQ,77
	JRST[	MOVEI MQ,77
		JRST DPIT]
	MOVN MQ,MQ
	CAILE MQ,100
	JRST[	MOVEI MQ,100
		JRST DPNIT]
DPNIT:	MOVN MQ,MQ
DPIT:	DPB MQ,MOTPTR(I)
	DATAO ARM,@MOTPTR(I)
	SUB P,[XWD 2,2]
	JRST @2(P)
MOTPTR:	POINT 7,MOTDAT,6
	POINT 7,MOTDAT,13
	POINT 7,MOTDAT,20
	POINT 7,MOTDAT,27
	POINT 7,MOTDAT,34
	POINT 7,MOTDAT+1,6
	POINT 7,MOTDAT+1,13
	POINT 7,MOTDAT+1,20
MOTDAT:	0
	1
MOTBRK:	0

MOTSTP:	MOVE MQ,MOTBRK
	TDZ MQ,BBIT(I)
	CONO ARM,(MQ)
	MOVEM MQ,MOTBRK
	MOVEI MQ,0
	DPB MQ,MOTPTR(I)
	DATAO ARM,@MOTPTR(I)
	POPJ P,

MOTSTA:	MOVE MQ,MOTBRK
	TDO MQ,BBIT(I)
	CONO ARM,(MQ)
	MOVEM MQ,MOTBRK
	POPJ P,
BBIT:	400000
	200000
	100000
	 40000
	 20000
	 10000
	  4000
	  2000
>
PREAD:
IFN TRACK,<
	SKIPE WALK
	JRST[GNPR:AOBJP DATA,[	MOVE AC,[IOWD DBL,DATO]
				MOVEM AC,DATPT
				MOVE DATA,DATPT
				SETZ TAC,
				INPUT 15,1
				JRST GNPR]
		MOVE DACVAL,(DATA)
		JRST @READFN]
>
	MOVEI TAC,10
	MOVEM TAC,DATMSD
DACRED:	SOSGE DATMSD
	JRST[REDER:	MOVEI TAC,READER	
			MOVEM TAC,TRAJER
			JRST TOFF]
DACST:	CONO DB,4250
	CONO AD,(K)
NREAD:	MOVEI TAC,30
	CONSO DB,1000
	SOJGE TAC,.-1
	JUMPL TAC,[DACDIE:CONO AD,4000(K)	;STOP DAC
			TRO REREAD
		IFN DEB,<MOVEM DACVAL,DACERR>
			MOVEI TAC,12
		WRH:	SOJGE TAC,WRH
			JRST DACRED]
	DATAI DB,DACVAL
	CONSZ DB,410000
	JRST[	CONSO DB,400000		;HAS THE RED BUTTON BEEN PUSHED?
		JRST DACDIE		;NO DATA MISSED GO TRY AGAIN
		MOVEI TAC,REDOFF
		MOVEM TAC,TRAJER
		JRST TOFF]
	XOR DACVAL,SBMSK
IFN TRACK,<PUSH DATA,DACVAL>
	JRST @READFN
FREAD:	ADDI K,770000
IFN TRACK,<SKIPN WALK>
	CONO AD,4000(K)	;STOP AD ON NEXT CHANNEL
	POPJ P,
READFN:	FREAD

	XLIST
	SUBTTL	CONSTANTS				LOU PAUL
	LIST
;DATA
TRANS:	BLOCK 14
THSOLN:	BLOCK 6
HDP:	0
HOMEC:	0
THM:	BLOCK 6
THF:	BLOCK 6
TIMM:	0
TIMF:	0
NON:	POINT 12,INNER(AC),11
	POINT 12,INNER(AC),23
	POINT 12,INNER(AC),35
	POINT 12,OUTER(AC),11
	POINT 12,OUTER(AC),23
	POINT 12,OUTER(AC),35
ERR:	0.05
	0.05
	0.01
	0.1
	0.1
	0.7
IFN SIMU,<
	TH-1
ARM.TH:	.+4
>
TH:		;ARM POSITION JUST READ
IFE BLUE,<-180.0> IFN BLUE,<180.0> ;THE NEXT 7 LOCATIONS ARE BLOCK TRANSFERED.
	-90.0
	12.0
	-90.0
	90.0
	0.0
IFN SIMU,<ARM.GR:>
HAND:	3.0
POT:	BLOCK 6
TDD:	BLOCK 6		;PREDICTED ACCELERATION
ERRINT:	0
	0
	0
	0
	0
	0
DTH:	BLOCK 6
TD:	BLOCK 6		;ACTUAL VELOCITY
IFN SMV,<
MAXVEL:	0
	0
	0
	0
	0
	0
MINVEL:	17777
	17777
	17777
	17777
	17777
	17777
>
T0P:	BLOCK 6		;PREVIOUS SET POINT
TDP:	BLOCK 6		;THETA DOT PAST
ET0:	BLOCK 6
T0:			;SET POINT
IFE BLUE,<-180.0> IFN BLUE,<180.0>
	-90.0
	14.0
	-90.0
	 90.0
	 0.0000
TFF:
IFE BLUE,<-180.0> IFN BLUE,<180.0>
	-90.0
	14.0
	-90.0
	 90.0
	 0.0000
DELTH:	BLOCK 6		;THIS AND THE NEXT ARE BLT ED TOGETHER
RESTT:	BLOCK 14	;THE CUMULATIVE RESTORE MATRIX
RESTF:	0		;TRUE IF A RESTORE HAS HAPPENED
DDTH:	0
	0
	0
	0
	0
	0
NTICKS:	0
DNAME:	BLOCK NSCEL
DTRANS:	BLOCK NSCEL*14
TORE:	BLOCK 6
T:	BLOCK 6
IFN REPORT,<
PBLK:	<SIXBIT/THETA/>+6
JER:	BLOCK 6
	<SIXBIT/VEL/>+6
VER:	BLOCK 6
	<SIXBIT/DAC/>+6
>
IFN USER+REPORT+TRACK+DEB,<MOTOR:	BLOCK 6>
FBI:	252502
VZ:	BLOCK 6
IFN TACH,<
VDTH:	BLOCK 6
>
THP:	BLOCK 6		;PAST ARM POSITION
STQ:	BLOCK 6
SDTQ:	0
ASTP:	0
ASTPP:	0
PC:	BLOCK 6
PCC:	BLOCK 6
DCI:	BLOCK 6
DCII:	BLOCK 6
CI:	0
	0
	0
	0
	0
	0
CII:
IFE BLUE,<
	1000.0
	1200.0
	100.0
	100.0
	100.0
	100.0
>
IFN BLUE,<
	1000.0
	1200.0
	100.0
	100.0
	100.0
	100.0
>
NC:	0
	0
	0
	0
	0
	0
NCC:
IFE BLUE,<
	1000.0
	1200.0
	100.0
	100.0
	100.0
	100.0
>
IFN BLUE,<
	1000.0
	1200.0
	100.0
	100.0
	100.0
	100.0
>
IFN DEB,<DACERR:	0>
REFTR:	0
ETIME:	=16667
TDF:	1.0
IFN DEB,<
ETIM:	0
ACSAV:	BLOCK 20
>
PDL:	DHO
	BLOCK PDLL
LTIME:	0
IFN SIMU,<ARM.FU:>
FUNCT:	0
ROT6:	0
TRAJER:	0
PAUSEF:	0
RETFLG:	0
RUNSEG:	0
DOING:	0
DATMSD:	0
CBUFS:	0
DATWDS:	0
SNUM:	POINT 12,DACVAL,23
SBMSK:	400040004000
REV:
IFE BLUE,<
	0
	0
	-1
	0
	-1
	-1
>
IFN BLUE,<
	-1
	0
	-1
	-1
	0
	-1
>
DMASK:	400000
	100000
	20000
	4000
	1000
	200
BMASK:	XWD 400000,200000
	XWD 200000, 40000
	XWD 100000, 10000
	XWD  40000,  2000
	XWD  20000,   400
	XWD  10000,   100
GMASK:	200000
	 40000
	 10000
	  2000
	   400
	   100
VCHAN:
IFE BLUE,<
	52B23
	53B23
	54B23
	0
	0
	0
>
IFN BLUE,<
	41B23
	42B23
	43B23
	44B23
	45B23
	0
>
CHAND:
IFE BLUE,<
	0
	0
	0
	0
	0
	XWD OFF5,CH5
>
IFN BLUE,<
	0
	0
	0
	XWD OFF3,CH3
	0
	XWD OFF5,CH5
>
CHAN:
IFE BLUE,<
	55B23
	56B23
	57B23
	60B23
	61B23
	62B23
>
IFN BLUE,<
	30B23
	31B23
	32B23
	33B23
	35B23
	36B23
>
THIRD:	0.3
SIXTH:	0.1
	BLOCK =20
	0
WAITIN:	XWD 3,0
JUMPBK:	XWD JMPABS,WAITIN
STKPTR:	WAITIN
ERRSTK:	0
BITSAV:	0
COUNT:	0
STWT:	0
HCNT:	0
TP:	0
	0
	0
	0
	0
	0
TJ:	BLOCK 6
LOOP:	BLOCK 10
TN:	BLOCK 6
SC:	BLOCK 6
A0:	BLOCK 6
A1:	BLOCK 6
A2:	BLOCK 6
A3:	BLOCK 6
A4:	BLOCK 6
BLOCKS:	0
NXTBUF:	0
BUF:	0
	BLOCK 1000
BUF2:	0
	BLOCK 1000
IFN ADJUST<
ADJI:	-1
ADJV:	0
ADJE:	0
ADJIF:	0
DL:	0
DH:	0
FACTOR:	BLOCK 3
>
IFN REPORT+TRACK,<
DATIWD:	0
	0
DATPT:	0
DATO:	BLOCK DBL+100
>
IFN DYNAMICS,<
DDAT:	<SIXBIT/FORD/>+6
FOR:	BLOCK 6
	<SIXBIT/BACK/>+6
BAK:	BLOCK 6
>
;FOR AN INCREMENT, USED FOR CHANGE
;	BEGINNING OF TRAJECTORY
;		DTH←TH-TFF,  T0←TH
;	DURING FOR H←0 STEP T/TN UNTIL 1 DO
;		T0←TFF(A CONSTANT)+DTH+H*DELTH, DELTH IS THE TOTAL CHANGE
;	END
;		DTH←T0-TFF, DELTH←0
;FOR A DROP, USED FOR PLACE AND CENTER
;	B:  	DTH←TH-TFF, T0←TH;
;	M:  	FOR H←0 STEP 1 UNTIL TN DO
;		T0←TFF(A CONSTANT)+DTH+DELTH, DELTH IS THE INCREMENTAL CHANGE
;	E:	DTH←T0+TFF,DELTH←0
;FOR A RUN, USED FOR MOVE
;	B:	DTH←TH-TFF, T0←TH
;	1ST SEG	T0←TFF(t/T)+DTH
;	MID SEG T0←TFF(t/T)+DTH+H*DELTH
;	E:	DTH←T0-TFF
;FOR A GOTO
;	B:	FIRST CHANGE IN DELTH TIME IN NTICKS
;	M:	SECOND CHANGE IN THM TIME IN TIMM
;	E:	THIRD CHANGE IN THF TIME IN TIMF
;DYNAMICS
IFN ADJUST,<
INL:	MOVE I,ADJI
	MOVE TAC,KV(I)
	MOVEM TAC,ADJV
	MOVE TAC,KE(I)
	MOVEM TAC,ADJE
	MOVE TAC,KI(I)
	MOVEM TAC,ADJIF
	JRST WAIT
>
IFE BLUE,<
PK:	0;	0.00033
	0;	0.00063
	0;	-0.026
	0;	-0.0009
	0;	0.0
	0;	0.06666
F0:	800.0
	320.0
	 76.0
	 80.0
	230.0
	120.0
V0:	0.6
	0.6
	3.0
	3.0
	3.0
	3.0
KM:	0.00187
	0.00191
	0.0822
	0.02543
	0.035
	0.06667
EMF:	4.35
	4.40
	33.0
	1.22
	1.22
	2.92
MOTARM:	400.0
	750.0
	50.0
	200.0
	200.0
	50.0
KV:	400.0
	300.0
	 70.0
	100.0
	 60.0
	100.0
KE:	60000.0
	150000.0
	1500.0
	8000.0
	8000.0
	1200.0
KI:	1200000.0
	3800000.0
	13000.0
	140000.0
	200000.0
	5000.0
VSCALE:	-13.3
	-10.8
	-1.02
>
IFN BLUE,<
V0:	1.0
	2.0
	3.0
	3.0
	3.0
	2.0
F0:	1180.0
	1333.0
	  42.3
	 351.0
	 223.0
	 300.0
KM:	0.00450
	0.00342
	0.211
	0.0534
	0.0738
	0.118
MOTARM:	400.0
	750.0
	50.0
	200.0
	200.0
	50.0
KV:	500.0
	800.0
	 20.0
	120.0
	 65.0
	120.0
KE:	125000.0
	125000.0
	500.0
	15000.0
	15000.0
	 800.0
KI:	2250000.0
	2250000.0
	   3000.0
	 500000.0
 	 500000.0
	  15000.0
VSCALE:	 12.68
	-12.93
	16.84
	-22.67
	22.18
>
	XLIST
	SUBTTL	ARM CALIBRATION				LOU PAUL 
	LIST
;NON LINEAR CALIBRATION
        RADIX = 10
IFE BLUE,<
INNER:  BYTE (12)   995, 979,1020
        BYTE (12)   995, 979,1016
        BYTE (12)   995, 979,1013
        BYTE (12)   995, 982,1010
        BYTE (12)   995, 994, 989
        BYTE (12)   991,1006, 999
        BYTE (12)   989,1021,1001
        BYTE (12)  1007,1031,1011
        BYTE (12)  1007,1008, 996
        BYTE (12)  1010, 983, 973
        BYTE (12)  1007, 972, 970
        BYTE (12)  1001, 972, 975
        BYTE (12)   995, 983, 941
        BYTE (12)   988,1008, 944
        BYTE (12)   986,1017, 989
        BYTE (12)   986,1017,1047
        BYTE (12)   986,1017,1073
OUTER:  BYTE (12)  1025,1061,1098
        BYTE (12)  1019,1061,1098
        BYTE (12)  1005,1061,1059
        BYTE (12)   979,1061,1014
        BYTE (12)   989,1047, 999
        BYTE (12)   996,1014, 980
        BYTE (12)  1007, 991, 962
        BYTE (12)  1013, 973, 947
        BYTE (12)  1004, 966, 948
        BYTE (12)   997, 959, 955
        BYTE (12)   997, 948, 961
        BYTE (12)   997, 951, 969
        BYTE (12)   997,1006, 988
        BYTE (12)   997,1066, 995
        BYTE (12)   997,1097,1015
        BYTE (12)   997,1097,1063
        BYTE (12)   997,1097,1087
        RADIX =8
USTOP:	60.0
	-60.0
	27.5
	145.0
	102.0
	275.0
LSTOP:	-185.0
	-185.0
	6.5
	-175.0
	-102.0
	-275.0
LDEL:	5.0
	10.0
	0.5
	5.0
	2.0
	5.0
SCALE:      1.233472
            0.847226
            0.092519
            1.341662
           -1.120682
           -1.408665
OFF:     -299.690800
     !5iqαqQ@4R↓↓↓↓α↓↓↓↓α↓↓I939]YeHh)↓↓α↓↓↓↓α↓5IM"qX4)α↓↓↓↓α↓↓↓↓I]9@hQ↓↓↓α↓↓↓↓α↓IUas4(%h(%Y∀⊃IL4PIYN	∪_4*∞C)h%Y∀⊃IL4PIYN	∪_4(%3∩	ILhP%@4PI5EA
q@4(J↓↓]Usλ4*>41Uh%β⊃Ua9h(%↓#→I9XhP%↓Y→9`4Sp4*&4qα
2,)1p4TJ::⊗∪Q↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4TzVR⊗∪Q↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓α
M"∃↓!⊃%↓↓↓AA1↓AA1↓A@4R↓↓↓↓α↓↓αJ"&a↓k@4*V≥">AhK	eA9h(%Us4(%≠!9H4PIIMUs4(%↓U9@hP%IIαq@4*e~R>API5eAs4(%k	aU9h(%Ys⊂4(%k→IU9h(%5↓U9@hP%5M+↓9@4Tb∩⊗1PIU9@hP%U9h(%As(4(%*q@4(K⊃9@4PIU9@hRN∞εd)i↓↓α↓↓↓Es⊃QT4R↓↓↓↓α↓↓↓↓α↓5E9β⊃@4)α↓↓↓↓α↓↓↓↓αiA9E≠→X4)α↓↓↓↓α↓↓↓↓α↓E9M≠(4)↓α↓↓↓↓α↓↓↓↓k	9AQ84)↓α↓↓↓↓α↓↓↓↓β	9ME# 4*>41i↓↓α↓↓5E#)9YDhQ↓↓↓α↓↓↓↓α↓↓]es→]`4R↓↓↓↓α↓↓↓↓α↓QA93A]T4R↓↓↓↓α↓↓↓↓k→YM9K(4)↓α↓↓↓↓α↓↓↓E;)9UXhQ↓↓↓α↓↓↓↓α↓↓UUsAIH4Ph(%@hP%MN∪⊃L4(K→R	I_h*∞!≠P%MN∪⊃L4(K→R	I_h(%M≤⊃IL4PI@4(K→Z	I_h(%M<⊃IL4T~!UhK→Z	I_h(%M<⊃IL4PIMZ	∪_4(%h(%↓αiE9dhP%5EC)9\4Tz~→MPI5MY*qT4(JiUUAs@4(%h(%↓α↓e9HhP%5EC↓9D4Tz~→UPI5MU
pctb5
	-540.8
	0
>
        XLIST
        SUBTTL	HAND FUNCTIONS				LOU PAUL
        LIST
HOPEN:				;OPENS THE HAND TO SET POINT
	TRNN HCL
	JRST[	MOVEI TAC,=240	;GO HERE FIRST TIME
		MOVEM TAC,HCNT	;MAX TIME TO SERVO THERE
		ADDI TAC,=60
		MOVEM TAC,COUNT
		HRLZ AC,@STKPTR	;REQUIRED OPENING
IFE BLUE,<	MOVE MQ,[3.95]>
IFN BLUE,<	MOVE MQ,[3.75]>
		CAML AC,MQ
		MOVE AC,MQ	;NOT GREATER THAN 2.5
		MOVEM AC,RSET	;REQUIRED FINAL SET POINT
IFE USER,<	PUSHJ P,HEAD	;READS THE HAND>
		MOVE AC,HAND	;AND STORE IT HERE
		MOVEM AC,SET	;DYNAMIC SET POINT
		TRZ STCH	;TURN OFF TOUCH AND SAVE STATE
		TRO HCL		;SO THAT WE DON'T COME BACK HERE
IFN BLUE,<	MOVEI I,6
		PUSHJ P,MOTSTA
>
		MOVE TAC,[0.1]
		CAML AC,RSET
		MOVN TAC,TAC
		MOVEM TAC,DIR	;CHANGE OF SET POINT PER JIFFY
		JUMPGE TAC,NXTJIF
		SKIPE SOTCH	;AND RESTORE TOUCH
		JRST[	TRO STCH
			FMPR TAC,[0.4]
			MOVEM TAC,DIR
			JRST NXTJIF]
		JRST NXTJIF]
HSU:	PUSHJ P, HEAD		;INCREMENTS SETPOINT AND SETS D/A
				;LEAVES ARM DATAO IN TAC
IFE HANDWORK <
	JRST NEXT
>
	SOSGE HCNT
	JRST[	MOVEI AC,HTERR
		MOVEM AC,TRAJER
		POP P,AC
		JRST TOFF]
	SKIPE DIR
	JRST DAW		;IF NOT AT FINAL SET DRIVE ANYWAY
	MOVM AC,HERR		;HAND ERROR
	FSBR AC,[0.05]
	JUMPG AC,DAW
	MOVM AC,HVERR		;HAND VELOCITY ERROR
	FSBR AC,[0.02]
	JUMPG AC,DAW
	TRZ HCL
IFE BLUE,<
	TDZ DATWD,HANDAT
>
IFN BLUE,<
	MOVEI I,6
	PUSHJ P,MOTSTP
>
	JRST NEXT
DAW:	
IFN TRACK,<SKIPN WALK>
IFE BLUE,<DATAO ARM,DATWD		;DRIVE HAND>
	JRST NXTJIF

HCLOSE:
	TRNN HCL		;ONLY ONCE
	JRST[	TRZ STCH	;TURN OFF TOUCH AND SAVE STATE
		MOVEI TAC,=240
		MOVEM TAC,HCNT	;MAX TIME TO SERVO THERE
		ADDI TAC,=60
		MOVEM TAC,COUNT
		HRLZ AC,@STKPTR	;MINIMUN OPENING
		MOVEM AC,MINDST
IFE USER,<	PUSHJ P,HEAD	;READ HAND>
		MOVE AC,HAND
		MOVEM AC,SET
		MOVE TAC,[-0.10]
		MOVEM TAC,DIR	;CHANGE OF SET POINT PER JIFFY
		MOVSI AC,(4.0)
		MOVNM AC,RSET	;FINAL SET POINT -1 SO AS TO SQUEEZE
		TRO HCL
IFN BLUE,<	MOVEI I,6
		PUSHJ P,MOTSTA
>
		JRST NXTJIF]
BB:	PUSHJ P,HEAD
IFE HANDWORK <
	JRST ER2
>
IFN TRACK,<SKIPN WALK>
IFE BLUE,<DATAO ARM,DATWD		;DRIVE HAND>
	MOVE AC,HERR
	CAMG AC,[1.00]
	JRST NXTJIF		;IF ERROR LESS THAN 1 KEEP ON
ER2:	TRZ HCL
IFE BLUE,<
	TDZ DATWD,HANDAT
>
IFN BLUE,< MOVEI I,6
	PUSHJ P,MOTSTP
>
	SKIPE SOTCH		;RESTORE TOUCH
	TRO STCH
	MOVE AC,MINDST
	CAMG AC,HAND
	JRST NEXT
	MOVEI 1,GRASER		;IF LESS THAN MINOPENING GIVE ERROR
	MOVEM 1,TRAJER
	POP P,1
	JRST  TOFF

HEAD:	MOVEI K,HANDCHA
IFE USER,<
	PUSHJ P,PREAD		;READ HAND CHANNEL
	LDB AC,SNUM
	ANDI DACVAL,7777
	ADDI AC,(DACVAL)	;ADD ALL THREE READINGS TOGETHER
	FSC AC,220
	FMPR AC,HSCALE
	FADR AC,HOFF
	MOVEM AC,HAND		;OPENING IN INCHES
>
IFN USER,<
	MOVE AC,FUNCT
	CAIN AC,1
	JRST[	SKIPN DIR
		JRST[	MOVE AC,RSET
			JRST SFH]
		MOVE AC,HAND
		FADR AC,DIR
		JRST SFH]
	MOVE AC,HAND
	FADR AC,DIR
	CAMG AC,MINDST
	MOVE AC,MINDST
SFH:	MOVEM AC,HAND
>
IFN REPORT,<
	PUSH DATA,[<SIXBIT/HAND/>+1]
	PUSH DATA,HAND
>
	SKIPN TAC,DIR
	JRST[	MOVE TAC,SET
		JRST GOTHER]
	FMPR TAC,TDF
	FADRB TAC,SET 		;INCREMENT SET POINT
	FSBR TAC,RSET	;NEGATIVE REMAINS TO GO
	FMPR TAC,DIR
	JUMPGE TAC,[	MOVE TAC,RSET	;GOT THERE
			MOVEM TAC,SET
			SETZM DIR
			JRST GOTHER]
	MOVE TAC,SET

;HAND SERVO

GOTHER:	FSBR AC,TAC		;AC = OBS-SET
	EXCH AC,HERR		;AC = OBS(-1)-SET(-1), HERR = OBS-SET
	FSBR AC,HERR		;AC = SET-SET(-1)-(OBS-OBS(-1)),= HVERR
	MOVEM AC,HVERR
	FMPR AC,KVH		;AC = KV*HVERR
	MOVN TAC,HERR		;TAC = HERR
	FMPR TAC,KEH		;TAC = KE*HERR
	FADR AC,TAC		;AC = FEEDBACK TERM
IFE BLUE,<
	TDO DATWD,HANDAT
	JUMPGE AC,.+2
	TRZ DATWD,40
	MOVM AC,AC
	FIX AC,211000
	CAILE AC,776000
	MOVEI AC,776000
	TRC AC,400000
	HRLI AC,6
IFN DEB<
	MOVEM AC,HANDAW
>
IFN TRACK,<SKIPN WALK>
	DATAO WIDTH,AC
>
IFN BLUE,<MOVEI I,6
	FIX AC,233000
	PUSH P,AC
	PUSHJ P,MOTDRV
>
	POPJ P,
DIR:	0			;CHANGE OF SETPOINT PER JIFFY
SET:	0			;DYNAMIC SET POINT
RSET:	0			;FINAL SET POINT
MINDST:	0			;MIN OPENING FOR CLOSE
HERR:	0			;HAND POSITION ERROR
HVERR:	0			;HAND VELOCITY ERROR
IFE BLUE,<
HANDAT:XWD 4000,60
HSCALE:	1.34
HOFF:	-0.58
KEH:	5.0			;ERROR GAIN
KVH:	10.0			;VELOCITY GAIN
>
IFN BLUE,<
HSCALE:	-1.277
HOFF:	4.09
KEH:	-65.0			;ERROR GAIN
KVH:	-300.0			;VELOCITY GAIN
>
IFN DEB<
HANDAW:	0
>

PLACE:				;PLACE HAND ON TABLE
	TRNN DROP		;ONLY COME HERE ONCE
	JRST[	TRO DROP+RUN
		MOVEI 1,=360
		MOVEM 1,COUNT
		HRRE 1,@STKPTR
		ADDI 1,(CBUF)
		HRLI 1,(1)
		HRRI 1,DELTH
		BLT 1,DELTH+5	;CHANGE OF THETA PER JIFFY
		MOVEI 1,=12
		MOVEM 1,HCNT	;DON'T STOP FOR THE FIRST 12 JIFFIES
		MOVE DATWD,[770000252502]
IFN BLUE,<	MOVEI I,5
	SFP:	PUSHJ P,MOTSTA
		SOJGE I,SFP
>
		HRRZM DATWD,FBI
		JRST NXTJIF]
	SOSL HCNT
	JRST NXTJIF
IFN USER,<JRST PLACED>
EXL:	MOVM AC,ET0+1
	CAML AC,PLER
	JRST PLACED
	JRST NXTJIF
PLACED:	SETZM FUNCT
	JRST RUDONE
PLER:	0.6

NUDGE:	PUSHJ P,ACSAVE
	PUSH P,[TRANS]
	PUSH P,[TH]
	PUSHJ P,UPDATE
	PUSH P,[TRANS]
	PUSH P,[TRANS]
	HRRE TAC,@STKPTR
	ADD TAC,CBUFS
	PUSH P,TAC
	PUSHJ P,TIMES
	PUSH P,[TRANS]
	MOVE AC,[XWD TH,THSOLN]
	BLT AC,THSOLN+5
	PUSH P,[THSOLN]
	PUSHJ P,SOLVE
	PUSHJ P,ACREST
	JUMPN 1,[	MOVEI AC,SOLERR
			MOVEM AC,TRAJER
			JRST TOFF]
	MOVEI I,5
NUL1:	MOVE AC,THSOLN(I)
	FSBR AC,TH(I)
	MOVEM AC,DELTH(I)
	SOJGE I,NUL1
	HRRE 1,@STKPTR
	ADDI 1,14(CBUF)
	PUSHJ P,UPDC
	HRRE 1,@STKPTR
	ADDI 1,14(CBUF)
	MOVE 1,7(1)
	ASH 1,16
	SKIPN 1
	JRST[	PUSH P,[DELTH]
		PUSHJ P,SLOTIM
		JRST .+1]
	MOVEM 1,NTICKS
	ASH 1,-15
	ADDI 1,=60
	MOVEM 1,COUNT
	TRO INCREM+RUN
	SETZM TICKS
	TRZE BOTH
	JRST[	AOS AC,STKPTR
		HLRE AC,(AC)
		MOVEM AC,FUNCT
		JRST .+2]
	SETZM FUNCT
	JRST NXTJIF

DRIVE:	HRRE TAC,@STKPTR
	ADDI TAC,(CBUF)
	HRLI TAC,(TAC)
	HRRI TAC,DELTH
	BLT TAC,DELTH+5
	HRRE 1,@STKPTR
	ADDI 1,6(CBUF)
	PUSHJ P,UPDC
	HRRE 1,@STKPTR
	ADDI 1,6(CBUF)
	MOVE 1,7(1)
	ASH 1,16
	SKIPN 1
	JRST[	PUSH P,[DELTH]
		PUSHJ P,SLOTIM
		JRST .+1]
	MOVEM 1,NTICKS
	ASH 1,-15
	ADDI 1,=60
	MOVEM 1,COUNT
	TRO INCREM+RUN
	SETZM TICKS
	TRZE BOTH
	JRST[	AOS AC,STKPTR
		HLRE AC,(AC)
		MOVEM AC,FUNCT
		JRST .+2]
	SETZM FUNCT
	JRST NXTJIF

STOP:	HRRE 1,@STKPTR
	ADDI 1,(CBUF)
	HRLI 1,(1)
	HRRI 1,STQ
	BLT 1,STQ+5
	MOVEI I,5
	SETZ AC,
DSTQ:	MOVE TAC,STQ(I)
	FDVR TAC,F0(I)
	MOVM MQ,TAC
	CAMGE MQ,[0.3]
	SETZB TAC,STQ(I)
	MOVEM TAC,STQ(I)
	FMPR TAC,TAC
	FADR AC,TAC
	SOJGE I,DSTQ
	MOVEM AC,SDTQ
	SETZM ASTPP
	TRO STP
	JRST NEXT

TOOL:	TRNN HCL
	JRST[	HRRE TAC,@STKPTR
IFE BLUE,<
		CAIL TAC,77
		MOVEI TAC,77
		MOVN TAC,TAC
		CAILE TAC,100
		MOVEI TAC,100
		MOVN TAC,TAC
		DPB TAC,[POINT 7,TDAT,20]
		CONO DEVT,1
		TRO HCL
		JRST .+1]
	SKIPG COUNT
	JRST[	SETZ AC,
		DPB AC,[POINT 7,TDAT,20]
		DATAO DEVT,TDAT
		TRZ HCL
		JRST NEXT]
	DATAO DEVT,TDAT
>
IFN BLUE,<
		MOVEM TAC,TDAT
		MOVEI I,7
		PUSHJ P,MOTSTA
		PUSH P,TDAT
		PUSHJ P,MOTDRV
		TRO HCL
		JRST .+1]
	MOVEI I,7
	SKIPG COUNT
	JRST[	PUSHJ P,MOTSTP
		TRZ HCL
		JRST NEXT]
	PUSH P,TDAT
	PUSHJ P,MOTDRV
>
	JRST NXTJIF
TDAT:	1
VOPEN:	MOVEI	TAC,1000
	JRST	.+2
VCLOSE:	MOVEI	TAC,2000
     	TRNN HCL
	JRST[	HRRE AC,@STKPTR		;GET LENGTH OF TIME FOR VICE OP.
		CAILE AC,=900		;ALLOW AT MOST 15 SECONDS
		MOVEI AC,=900
		MOVEM AC,COUNT		;ADJUST TIME OUT COUNT
		CONO DEVT,1		;OVER-RIDE BLUE ARM SAFETY
		TRO HCL			;ONLY DO THIS SECTION OF CODE ONCE
		JRST .+1]
	CONO DEVT,(TAC)			;START MOVING VICE
	DATAO DEVT,
	SKIPG COUNT
	JRST[	CONO DEVT,		;STOP MOVING VICE
		TRZ HCL
		JRST NEXT]
	JRST NXTJIF
PATH:	TRO RUN
	AOS DOING	;INCREASE TRAJECTORY NUMBER
	MOVEI AC,=1800
	MOVEM AC,COUNT
	SETZM LOOP
	MOVE TAC,[XWD LOOP,LOOP+1]
	BLT TAC,LOOP+9
	MOVEI I,5
	HRRE K,@STKPTR
	ADDI K,34
SUL:	SUBI K,4
	HRLZM K,TJ(I)
	HRLZM K,TN(I)
	MOVEI TAC,TJ(I)
	MOVEM TAC,TP(I)
	SOJGE I,SUL
	SETZM ETIME
	PUSHJ P,EVAL
	TRZ INCREM
	HRRE TAC,@STKPTR
	ADDI TAC,(CBUF)
	HRRZ AC,(TAC)
	ASH AC,16
	MOVEM AC,NTICKS	;RUN TIME
	SETZM TICKS
	MOVEI I,5
MOVBK:	MOVE AC,TH(I)
	MOVEM AC,T0(I)
	FSBR AC,TFF(I)
	MOVEM AC,DTH(I)
	MOVN AC,AC
	SKIPE RESTF
	FADR AC,DDTH(I)
	MOVEM AC,DELTH(I)
	SETZM TD(I)
	SOJGE I,MOVBK
	SKIPE RESTF
	JRST[	MOVE AC,[XWD RESTF,DDTH]
		SETZM RESTF
		BLT AC,DDTH+5
		JRST .+1]
	SETZM TICKS
	TRZE BOTH
	JRST[	AOS AC,STKPTR
		HLRE AC,(AC)
		MOVEM AC,FUNCT
		JRST .+2]
	SETZM FUNCT
	JRST NXTJIF

ASSIGN:	HRRE K,@STKPTR
	ADDI K,(CBUF)
	MOVE TAC,(K)
	MOVEI I,NLCEL-1
ASS1:	MOVE AC,LNAME(I)
	CAME AC,TAC
	SOJGE I,ASS1
	JUMPGE I,FONEL
	MOVEI I,NLCEL-1
ASS2:	SKIPE LNAME(I)
	SOJGE I,ASS2
	JUMPL I,[MOVEI TAC,SOJERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	MOVEM TAC,LNAME(I)
FONEL:	AOJ K,
	MOVE AC,(K)
	MOVEM AC,LCOUNT(I)
	JRST NEXT

LCOUNT:	BLOCK NLCEL
LNAME:	BLOCK NLCEL

SAVE:	HRRE K,@STKPTR
	ADDI K,(CBUF)
	MOVE TAC,(K)
	MOVEI I,NSCEL-1
SAV1:	MOVE AC,DNAME(I)
	CAME AC,TAC
	SOJGE I,SAV1
	JUMPGE I,FONE
	MOVEI I,NSCEL-1
SAV2:	SKIPE DNAME(I)
	SOJGE I,SAV2
	JUMPL I,[MOVEI TAC,SAVERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	MOVEM TAC,DNAME(I)
FONE:	IMULI I,=12
	ADDI I,DTRANS
	PUSH P,I
	PUSHJ P,ACSAVE
	PUSH P,[TRANS]
	PUSH P,[TH]
	PUSHJ P,UPDATE
	PUSH P,[TRANS]
	PUSH P,[TRANS]
	HRRE K,@STKPTR
	ADD K,CBUFS
	AOJ K,
	PUSH P,K
	PUSHJ P,TIMES
	PUSHJ P,ACREST
	POP P,TAC
	MOVEI AC,13(TAC)
	HRLI TAC,TRANS
	BLT TAC,(AC)
	JRST NEXT

RESTORE:HRRE K,@STKPTR
	ADDI K,(CBUF)
	MOVE TAC,(K)
	MOVEI I,NSCEL-1
RES1:	MOVE AC,DNAME(I)
	CAME AC,TAC
	SOJGE I,RES1
	JUMPL I,[MOVEI TAC,SAVERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	SKIPE 1(K)
	SETZM DNAME(I)
	PUSHJ P,ACSAVE
	IMULI I,=12
	ADDI I,DTRANS
	ADDI K,2
	PUSH P,[TRANS]
	PUSH P,[RESTT]
	PUSH P,K
	SKIPE RESTF
	JRST[	PUSH P,[RESTT]
		PUSH P,I
		PUSH P,[RESTT]
		PUSHJ P,TIMES
		JRST RES7]
	SETOM RESTF
	HRL AC,I
	HRRI AC,RESTT
	BLT AC,RESTT+13
RES7:	PUSHJ P,TIMES
	PUSH P,[TRANS]
	HRRE K,@STKPTR
	ADD K,CBUFS
	HRLI AC,16(K)
	HRRI AC,THSOLN
	BLT AC,THSOLN+5
	PUSH P,[THSOLN]
	PUSHJ P,SOLVE
	PUSHJ P,ACREST
	JUMPN AC,[MOVEI AC,SOLERR
		MOVEM AC,TRAJER
		JRST TOFF]
	MOVEI I,5
	HRRE K,@STKPTR
	ADDI K,23(CBUF)
RES2:	MOVE AC,THSOLN(I)
	FSBR AC,(K)
	MOVEM AC,DDTH(I)
	SOJ K,
	SOJGE I,RES2
	JRST NEXT

TIMFAC: 0.5
	0.5
	4.5
	0.2
	0.2
	0.4

SET.ARM:HRRE K,@STKPTR
	ADDI K,(CBUF)
	MOVE TAC,(K)
	MOVEI I,NSCEL-1
SET1:	MOVE AC,DNAME(I)
	CAME AC,TAC
	SOJGE I,SET1
	JUMPGE I,FSET
	MOVEI I,NSCEL-1
SET2:	SKIPE DNAME(I)
	SOJGE I,SET2
	JUMPL I,[MOVEI TAC,SAVERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	MOVEM TAC,DNAME(I)
FSET:	IMULI I,=12
	ADDI I,DTRANS
	MOVEI AC,13(I)
	HRLI I,1(K)
	BLT I,(AC)
	JRST NEXT

CENTER:	TRNN HCL
	JRST[	TRO HCL
		PUSHJ P,ACSAVE
		PUSH P,[TRANS]
		PUSH P,[TH]
		PUSHJ P,UPDATE
		MOVE AC,TRANS+1
		FADRM AC,TRANS+3
		MOVE AC,TRANS+5
		FADRM AC,TRANS+7
		MOVE AC,TRANS+11
		FADRM AC,TRANS+13
		PUSH P,[TRANS]
		MOVE AC,[XWD TH,DELTH]
		BLT AC,DELTH+5
		PUSH P,[DELTH]
		PUSHJ P,SOLVE
		JUMPN 1,[ MOVEI AC,SOLERR
			MOVEM AC,TRAJER
			JRST TOFF]
		PUSHJ P,ACREST
		MOVEI I,5
	CE1:	MOVE AC,DELTH(I)
		FSBR AC,TH(I)
		FMPR AC,[0.005]
		MOVEM AC,DELTH(I)
		SOJGE I,CE1
		MOVEI TAC,=600
		MOVEM TAC,COUNT
		HRLZ AC,@STKPTR	;MINIMUN OPENING
		MOVEM AC,MINDST
		MOVSI AC,(4.0)
		MOVNM AC,RSET
IFN BLUE,<	MOVEI I,6
		PUSHJ P,MOTSTA
>
IFE USER,<	PUSHJ P,HEAD>
		MOVE AC,HAND
		MOVEM AC,SET
		TRZ STCH
		MOVE TAC,[-0.02]
		MOVEM TAC,DIR
		JRST NXTJIF]
	PUSHJ P,HEAD
IFE HANDWORK,<JRST ER2>
IFE BLUE,<DATAO ARM,DATWD>
	TRNN DROP
	JRST[	MOVEI I,1
	CFT:	MOVE AC,OBS(I)
		JUMPG AC,[SETZM TOT
			SKIPN I
			AOS TOT
			TRO DROP+RUN
			MOVE TAC,[-0.01]
			MOVEM TAC,DIR
			SKIPN TOT
			JRST[	MOVEI I,5
			OT:	MOVE AC,DELTH(I)
				MOVNM AC,DELTH(I)
				SOJGE I,OT
				JRST C2]
		C2:	MOVE TAC,[XWD 770000, 252500]
IFN BLUE,<
			MOVEI I,5
		CSA:	PUSHJ P,MOTSTA
			SOJGE I,CSA
>
			HRRZM TAC,FBI
			IOR DATWD,TAC
			MOVSI AC,(1.0)
			MOVEM AC,TDF
			MOVEI I,4
		GETGO:	PUSHJ P,NXTH
			SOJGE I,GETGO
			JRST NXTJIF]
		SOJGE I,CFT
		JRST NXTJIF]
	MOVE TAC,TOT
	SKIPG OBS(TAC)
	JRST NXTJIF
	TRZ DROP+RUN+FINAL
	PUSHJ P,SETSET
IFN BLUE,<
	MOVEI I,5
	PUSHJ P,MOTSTP
	SOJGE I,.-1
>
	TDZ DATWD,[XWD 770000,252500]
	MOVE TAC,DIR
	FSC TAC,3
	MOVEM TAC,DIR
	MOVEI TAC,2
	MOVEM TAC,FUNCT
	JRST NXTJIF]
TOT:	0

WOBBLE:	HRLZ AC,@STKPTR
	MOVEM AC,WOBMAG
	MOVEI AC,1
	MOVEM AC,WOBCNT+2
	MOVEM AC,WOBCNT+1
	MOVEI AC,6
	MOVEM AC,WOBCNT
	MOVN AC,WOBMAG
	FADRM AC,DTH+3
	TRO WOB
	JRST NEXT
WOBMAG:	0
WOBCNT:	BLOCK 3
SIN:	 0.0
	 0.30902
	 0.58779
	 0.80902
	 0.95106
	 1.0
	 0.95106
	 0.80902
	 0.58779
	 0.30902
	 0.0
	-0.30902
	-0.58779
	-0.80902
	-0.95106
	-1.0
	-0.95106
	-0.80902
	-0.58779
	-0.30902
SEA:	SETZM XSEL
	SETZM SCOUNT
	SETZM TCOUNT
	HRRE 1,@STKPTR
	ADDI 1,(CBUF)
	HRLI 1,(1)
	HRRI 1,XDEL
	BLT 1,AOJJ+5
	SETZM SDTH
	MOVE AC,[XWD SDTH,SDTH+1]
	BLT AC,SDTH+2
	JRST NEXT

AOJI:	MOVEI I,2
	MOVE K,SCOUNT
	SOSG J,TCOUNT
	JRST[	SKIPN XSEL
		AOS K,SCOUNT
		SETCMM XSEL
		MOVEI J,(K)
		MOVEM J,TCOUNT
		JRST .+1]
	SKIPE XSEL
	JRST[IX:MOVE AC,XDEL(I)
		TRNN K,1
		MOVN AC,AC
		FADRB AC,SDTH(I)
		SOJGE I,IX
		JRST NJ]
IY:	MOVE AC,YDEL(I)
	TRNN K,1
	MOVN AC,AC
	FADRB AC,SDTH(I)
	SOJGE I,IY
NJ:	MOVE AC,SDTH
	MOVEM AC,AOJT+3
	MOVE AC,SDTH+1
	MOVEM AC,AOJT+7
	MOVE AC,SDTH+2
	MOVEM AC,AOJT+13
	PUSHJ P,ACSAVE
	PUSH P,[TRANS]
	PUSH P,[RESTT]
	PUSH P,[AOJZ]
	SKIPE RESTF
	JRST[	PUSH P,[RESTT]
		PUSH P,[AOJT]
		PUSH P,[RESTT]
		PUSHJ P,TIMES
		JRST SE7]
	SETOM RESTF
	MOVE AC,[XWD AOJT,RESTT]
	BLT AC,RESTT+13
SE7:	PUSHJ P,TIMES
	PUSH P,[TRANS]
	MOVE AC,[XWD AOJJ,THSOLN]
	BLT AC,THSOLN+5
	PUSH P,[THSOLN]
	PUSHJ P,SOLVE
	PUSHJ P,ACREST
	JUMPN 1,[MOVEI AC,SOLERR
		MOVEM AC,TRAJER
		JRST TOFF]
	MOVEI I,5
SE2:	MOVE AC,THSOLN(I)
	FSBR AC,AOJJ(I)
	MOVEM AC,DDTH(I)
	SOJGE I,SE2
	HRRE TAC,@STKPTR
	ADD TAC,STKPTR
	SOJ TAC,
	HRRM TAC,STKPTR
	JRST NEXT

TCOUNT:	0
SCOUNT:	0
XSEL:	0
XDEL:	BLOCK 3
YDEL:	BLOCK 3
AOJZ:	BLOCK 14
AOJJ:	BLOCK 6
AOJT:	1.0
	0
	0
	0
	0
	1.0
	0
	0
	0
	0
	1.0
	0
SDTH:	BLOCK 3

SOJGI:	HRRE K,@STKPTR
	ADDI K,(CBUF)
	MOVE TAC,(K)
	MOVEI I,NLCEL-1
SOJ1:	MOVE AC,LNAME(I)
	CAME AC,TAC
	SOJGE I,SOJ1
	JUMPL I,[MOVEI TAC,SOJERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	SOSG LCOUNT(I)
	JRST NEXT
	AOJ K,
	HRRE TAC,(K)
	ADD TAC,STKPTR
	SOJ TAC,
	HRRM TAC,STKPTR
	JRST NEXT
MOVING: HRRE K,@STKPTR
	ADDI K,(CBUF)
	MOVE TAC,(K)
	MOVEI I,NSCEL-1
MOV1:	MOVE AC,DNAME(I)
	CAME AC,TAC
	SOJGE I,MOV1
	JUMPGE I,FMOV
	MOVEI I,NSCEL-1
MOV2:	SKIPE DNAME(I)
	SOJGE I,MOV2
	JUMPL I,[MOVEI TAC,SAVERR
		MOVEM TAC,TRAJER
		JRST TOFF]
	MOVEM TAC,DNAME(I)
FMOV:	IMULI I,=12
	ADDI I,DTRANS
	MOVEM I,MOVT
	MOVEI AC,13(I)
	HRLI I,MOVIT
	BLT I,(AC)
	SETZM MOVTIM
	AOJ K,
	HRLI TAC,(K)
	HRRI TAC,MOVV
	BLT TAC,MOVV+2
	JRST NEXT

MOVTIM:	0
MOVT:	0
MOVV:	BLOCK 3
MOVIT:	1.0
	0
	0
	0
	0
	1.0
	0
	0
	0
	0
	1.0
	0
HOME:	HRRE AC,@STKPTR
	ADDI AC,(CBUF)
	MOVEM AC,HDP		;POINTS TO DATA
	PUSHJ P,ACSAVE
	PUSH P,[TRANS]
	PUSH P,[TH]
	PUSHJ P,UPDATE
	PUSH P,[TRANS]
	PUSH P,[TRANS]
	PUSH P,HDP
	PUSHJ P,TIMES		;LIFT OFF POINT
	PUSH P,[TRANS]
	MOVE AC,[XWD TH,DELTH]
	BLT AC,DELTH+5
	PUSH P,[DELTH]
	PUSHJ P,SOLVE		;ARM SOLUTION FOR LIFTOFF
	MOVEI AC,14
	ADDB AC,HDP		;LIFT OFF TIME
	MOVE TAC,(AC)
	ASH TAC,16
	MOVEM TAC,NTICKS
	AOS AC,HDP		;SET DOWN MOD MATRIX
	PUSH P,[TRANS]
	PUSH P,[RESTT]
	PUSH P,AC
	MOVEI AC,14
	ADDB AC,HDP		;MID TIME
	MOVE TAC,(AC)
	ASH TAC,16
	MOVEM TAC,TIMM
	AOS AC,HDP		;FINAL TRANS
	SKIPE RESTF
	JRST[	PUSH P,[RESTT]
		PUSH P,[RESTT]
		PUSH P,AC
		PUSHJ P,TIMES
		SETZM RESTF
		JRST H7]
	HRLI AC,(AC)
	HRRI AC,RESTT
	BLT AC,RESTT+13
H7:	PUSHJ P,TIMES
	MOVE AC,[XWD DELTH,TFF]
	BLT AC,TFF+5
	PUSH P,[RESTT]
	PUSH P,[TFF]
	PUSHJ P,SOLVE		;ARM SOLUTION FOR FINAL POINT
	PUSHJ P,ACREST
	JUMPN 1,[MOVEI AC,SOLERR
		MOVEM AC,TRAJER
		JRST TOFF]
	MOVEI AC,14
	ADDB AC,HDP		;FINAL TIME
	MOVE TAC,(AC)
	ASH TAC,16
	MOVEM TAC,TIMF
	MOVE AC,[XWD TFF,THM]
	BLT AC,THM+5
	PUSH P,[TRANS]
	PUSH P,[THM]
	PUSHJ P,SOLVE
	MOVEI I,5
H1:	MOVE AC,TFF(I)
	FSBR AC,THM(I)
	MOVEM AC,THF(I)
	MOVE AC,THM(I)
	FSBR AC,DELTH(I)
	MOVEM AC,THM(I)
	MOVE AC,DELTH(I)
	FSBR AC,TH(I)
	MOVEM AC,DELTH(I)
	MOVE AC,T0(I)
	FSBR AC,TFF(I)
	MOVEM AC,DTH(I)
	SOJGE I,H1

	SKIPN AC,NTICKS
	JRST[	PUSH P,[DELTH]
		PUSHJ P,SLOTIM
		MOVEM AC,NTICKS
		JRST .+1]
	ASH AC,-15
	MOVEM AC,COUNT
	SKIPN TIMM
	JRST[	PUSH P,[THM]
		PUSHJ P,RUNTIM
		MOVEM AC,TIMM
		JRST .+1]
	SKIPN TIMF
	JRST[	PUSH P,[THF]
		PUSHJ P,SLOTIM
		MOVEM AC,TIMF
		JRST .+1]
	MOVEI AC,2
	MOVEM AC,HOMEC
	AOS AC,HDP
	PUSHJ P,UPDC
	TRO INCREM+RUN+HOMEF
	SETZM TICKS
	MOVE AC,STKPTR
	TRZE BOTH
	JRST[	AOS AC,STKPTR
		HLRE AC,(AC)
		MOVEM AC,FUNCT
		JRST .+2]
	SETZM FUNCT
	JRST NXTJIF

	
RUNTIM:	SETZ AC,
	SKIPA
SLOTIM:	MOVE AC,[30.0]
	MOVE J,-1(P)		;ADDR OF ARRAY
	MOVEI I,5
R1:	MOVM MQ,5(J)
	FMPR MQ,TIMFAC(I)
	CAML MQ,AC
	MOVE AC,MQ
	SOJ J,
	SOJGE I,R1
	FADR AC,[20.0]
	FIX AC,215000
	SUB P,[XWD 2,2]
	JRST @2(P)

IKU:	HRRE AC,@STKPTR
	ADDI AC,(CBUF)
	MOVEM AC,HDP		;POINTS TO DATA
	PUSHJ P,ACSAVE
	SKIPE RESTF
	JRST[	PUSH P,[RESTT]
		PUSH P,[RESTT]
		PUSH P,HDP
		PUSHJ P,TIMES
		PUSH P,[RESTT]
		SETZM RESTF
		JRST I7]
	PUSH P,HDP
I7:	MOVE AC,[XWD TH,DELTH]
	BLT AC,DELTH+5
	SETZM DELTH+5
	MOVE AC,[-90.0]
	MOVEM AC,DELTH+3
	PUSH P,[DELTH]
	PUSHJ P,SOLVE		;ARM SOLUTION
	PUSHJ P,ACREST
	JUMPN 1,[MOVEI AC,SOLERR
		MOVEM AC,TRAJER
		JRST TOFF]
	MOVE AC,[XWD DELTH,TFF]
	BLT AC,TFF+5
	MOVEI I,5
I1:	MOVE AC,DELTH(I)
	FSBR AC,TH(I)
	MOVEM AC,DELTH(I)
	MOVE AC,T0(I)
	FSBR AC,TFF(I)
	MOVEM AC,DTH(I)
	SOJGE I,I1

	MOVEI AC,14
	ADDB AC,HDP		;TIME
	MOVE AC,(AC)
	ASH AC,16
	SKIPN AC
	JRST[	PUSH P,[DELTH]
		PUSHJ P,SLOTIM
		JRST .+1]
	MOVEM AC,NTICKS
	ASH AC,-15
	ADDI AC,=60
	MOVEM AC,COUNT
	AOS AC,HDP
	PUSHJ P,UPDC
	TRO INCREM+RUN
	SETZM TICKS
	MOVE AC,STKPTR
	TRZE BOTH
	JRST[	AOS AC,STKPTR
		HLRE AC,(AC)
		MOVEM AC,FUNCT
		JRST .+2]
	SETZM FUNCT
	JRST NXTJIF

REFLEX:	MOVEI I,1
IFE BLUE,<MOVEI K,65B23>
IFN BLUE,<MOVEI K,47B23>
REX:
IFE USER,<
	PUSHJ P,PREAD
	LDB AC,[POINT 12,DACVAL,11]
	ADD AC,REF(I)
	MOVEM AC,OBS(I)
>
RMOR:	SOJGE I,REX
	TRNN STCH
	POPJ P,
	MOVEI I,1
SIT:	MOVE AC,OBS(I)
	JUMPG AC,[	LSH I,3
			IORI I,TOUCH
			MOVEM I,TRAJER
			POP P,I
			JRST TOFF]
	SOJGE I,SIT
	POPJ P,

REF:	-1000
	-1000
	BLOCK 4
IFN SIMU,<
	OBS-1
ARM.TO:	.+4
>
OBS:	0
	0
SOTCHS:	0
SETCH:	MOVE AC,SOTCH
	MOVEM AC,SOTCHS
	HRLE AC,@STKPTR
	MOVEM AC,SOTCH
	JUMPE AC,NEXT
	TRO STCH
	JRST NEXT

RETURN:	SETOM RETFLG
	JRST PROCEED
PAUSE:	SETOM PAUSEF
	SKIPA
PROCEED:AOS RUNSEG
	MOVEI TAC,3
	MOVEM TAC,FUNCT
	TRO PROFLG
SETBK:	MOVE TAC,STKPTR
	CAIG TAC,BUF
	JRST PMDSTK
	AOJ TAC,
	HRRM TAC,JUMPBK
PMDSTK:	MOVEI TAC,WAITIN
	MOVEM TAC,STKPTR
IFN REPORT,<PUSH DATA,[0]>
	TRO DTST
	PUSHJ P,SWDONE
	XLIST
	LIT
	LIST
IFE SIMU,<END START>
IFN SIMU,<END>